diff --git a/.gitignore b/.gitignore index 63f3e9f..7e5c6e3 100644 --- a/.gitignore +++ b/.gitignore @@ -16,3 +16,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/Dockerfile b/Dockerfile index dac8f91..aa5593e 100644 --- a/Dockerfile +++ b/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\ 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 new file mode 100644 index 0000000..d7ed7cc --- /dev/null +++ b/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. \ No newline at end of file diff --git a/README.md b/README.md index e445125..1e269f1 100644 --- a/README.md +++ b/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 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/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/Tools/ThermalCamTemp.py b/Tools/ThermalCamTemp.py new file mode 100644 index 0000000..96111a7 --- /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': 25.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/README.md b/ros2_ws/README.md index 5bd5f15..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. @@ -99,10 +101,36 @@ 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 +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 (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 - Top-level quick start: `../README.md` diff --git a/ros2_ws/requirements.txt b/ros2_ws/requirements.txt index 3a36cd6..ac32ac5 100644 --- a/ros2_ws/requirements.txt +++ b/ros2_ws/requirements.txt @@ -2,4 +2,8 @@ pyserial pymavlink dronekit smbus -flirpy \ No newline at end of file +flirpy +numpy<2 +opencv-python +geopy +future 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..830c7f3 100644 --- a/ros2_ws/src/embr/config/sensors.json +++ b/ros2_ws/src/embr/config/sensors.json @@ -7,11 +7,31 @@ "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", "device": "/dev/ttyAMA1", "baud": 57600 + }, + "thermal": { + "mode": "real", + "params": { + "model": "3.1R", + "altitude_m": 0.086, + "pitch_deg": 0, + "display_width": 640, + "display_height": 480, + "colormap": "INFERNO", + "min_temp_c": 10.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 4e47b00..424ee12 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,8 +10,28 @@ "pattern": "hover" } }, - - "mavlink": { + "thermal": { + "mode": "real", + "params": { + "model": "3.1R", + "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/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/getCube.py b/ros2_ws/src/embr/embr/getCube.py old mode 100755 new mode 100644 index ec562e8..8fe9520 --- a/ros2_ws/src/embr/embr/getCube.py +++ b/ros2_ws/src/embr/embr/getCube.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,43 @@ 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/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/embr/hotspotLocator.py b/ros2_ws/src/embr/embr/hotspotLocator.py new file mode 100644 index 0000000..a40144b --- /dev/null +++ b/ros2_ws/src/embr/embr/hotspotLocator.py @@ -0,0 +1,335 @@ +#!/usr/bin/env python3 +""" +Thermal Hotspot GPS Locator (ROS2) + +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 +from embr.sensors import SensorFactory, SensorConfig, create_sensor + +import numpy as np +import cv2 +from geopy.distance import distance as geopy_distance +from geopy.point import Point + + +class hotspotLocator(Node): + def __init__(self): + super().__init__('thermal_hotspot_locator') + + # 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 = {} + + # 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) + 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 based on config + 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 + + # 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(f'hotspotLocator started (camera model: {self.lepton_model})') + + 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().warning("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 floats in degrees) + self.current_gps = (float(msg.lat), float(msg.lon)) + self.have_gps = True + + # 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(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): + self.get_logger().warning('Waiting for GPS fix...') + self._warned_waiting_gps = True + return + + # Determine image shape from layout + h = msg.layout.dim[0].size + w = msg.layout.dim[1].size + + rad = np.array(msg.data, dtype=np.uint16) + 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: + 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) + + # -------------------- 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 + + # 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 + 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') + self.get_logger().info(f"hotspots found: {len(hotspots)}") + + for blob in hotspots[:10]: + 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_deg) + + info = { + '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_deg, + 'distance_m': ground_dist, + } + + if ground_dist < min_dist: + min_dist = ground_dist + 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']:.5f}m, " + f"max={data['max_temperature_c']:.1f}C, size={data['size_pixels']}px" + ) + + +def main(args=None): + rclpy.init(args=args) + node = hotspotLocator() + 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/radio.py b/ros2_ws/src/embr/embr/radio.py index 2e9c31e..67c4636 100644 --- a/ros2_ws/src/embr/embr/radio.py +++ b/ros2_ws/src/embr/embr/radio.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 @@ -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 ) @@ -78,21 +78,26 @@ 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.radio_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.radio_connection.send_gps(lat_i, lon_i, alt_i, vel_i) except Exception as e: self.get_logger().error(f'Error sending GPS: {e}') def destroy_node(self): """Cleanup connection on shutdown.""" - if hasattr(self, 'radio_connection'): + if hasattr(self, 'mavlink_connection'): self.radio_connection.stop() super().destroy_node() 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/cube.py b/ros2_ws/src/embr/embr/sensors/cube.py index c67d709..88873b4 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}") @@ -77,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 @@ -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: 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..4de47cc --- /dev/null +++ b/ros2_ws/src/embr/embr/sensors/thermal.py @@ -0,0 +1,183 @@ +"""Thermal camera sensor implementations.""" + +import time +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.""" + + +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' + 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.""" + 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 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. + + 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 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 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 new file mode 100644 index 0000000..1e01fc6 --- /dev/null +++ b/ros2_ws/src/embr/embr/thermalStream.py @@ -0,0 +1,489 @@ +#!/usr/bin/env python3 +""" +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 std_msgs.msg import UInt16MultiArray, MultiArrayDimension, MultiArrayLayout +from msg_interface.msg import GPSAndIMU +from embr.sensors import create_sensor, SensorConfig, SensorFactory +import cv2 +import numpy as np +import subprocess +import threading +import queue + + +class ThermalStreamNode(Node): + def __init__(self): + super().__init__('thermal_stream_node') + + # 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 = params.get('display_width', 640) + self.display_height = params.get('display_height', 480) + # use a reddish/inferno colormap for thermal-style colors + 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() + + # Publisher: 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( + GPSAndIMU, + 'gps', + self.gps_callback, + 10 + ) + + # Initialize camera using sensor abstraction + self.get_logger().info('Initializing thermal camera...') + try: + 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 + + # FFmpeg process for HDMI streaming + self.ffmpeg_process = None + # 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 + 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""" + # Initialize direct framebuffer writer (faster and lower latency than ffmpeg) + try: + 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: + # 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 (useful for debugging) + 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 + """ + # 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) + + # 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 (efficient, on radiometric data) + hot_mask = temp_celsius > self.temp_threshold + + if np.any(hot_mask): + # 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) < 3: + 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 region_temps.size == 0: + continue + + max_temp = float(np.max(region_temps)) + + # 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_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 + + def _capture_and_publish(self): + """Capture once and publish temperature array""" + try: + # 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') + 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 (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 using sensor abstraction + try: + if hasattr(self, 'camera'): + self.camera.stop() + 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 7e7da7e..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 @@ -47,5 +47,23 @@ def generate_launch_description(): parameters=[{ 'config_file': config_file, }] + ), + Node( + package='embr', + executable='thermalStream', + name='thermal_stream', + output='screen', + emulate_tty=True, + parameters=[{ + 'config_file': config_file, + }] + ), + Node( + package='embr', + executable='hotspotLocator', + name='hotspot_locator', + parameters=[{ + 'config_file': config_file, + }] ) ]) diff --git a/ros2_ws/src/embr/package.xml b/ros2_ws/src/embr/package.xml index 0e3aa1f..e8e4921 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 pytest diff --git a/ros2_ws/src/embr/setup.py b/ros2_ws/src/embr/setup.py index ea861cd..9b54ec9 100644 --- a/ros2_ws/src/embr/setup.py +++ b/ros2_ws/src/embr/setup.py @@ -25,6 +25,8 @@ 'console_scripts': [ 'getCube = embr.getCube:main', 'getTemp = embr.getTemp:main', + 'thermalStream = embr.thermalStream:main', + 'hotspotLocator = embr.hotspotLocator:main', 'radio = embr.radio:main' ], }, diff --git a/ros2_ws/src/embr/test/test_sensors.py b/ros2_ws/src/embr/test/test_sensors.py index bb4b165..6cdf892 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,99 @@ 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_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 +259,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() 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..4695459 --- /dev/null +++ b/ros2_ws/src/msg_interface/msg/GPSAndIMU.msg @@ -0,0 +1,7 @@ +float32 lat +float32 lon +float32 alt +float32 vel +float32 pitch +float32 yaw +float32 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