Showing posts with label robotics. Show all posts
Showing posts with label robotics. Show all posts

Thursday

Raspberry Pi 5 16GB RAM as the central brain for an autonomous disaster recovery vehicle

To make this complex system actually work in a life-or-death disaster scenario, you must strategically optimize your software stack and offload specific tasks.

1. The AI Stack: Swapping vLLM for llama.cpp / Ollama

  • The Problem with vLLM: While vLLM is an incredible inference engine, it is heavily optimized for high-throughput enterprise server GPUs and relies on massive memory allocations (vLLM PagedAttention). Running vLLM on an ARM CPU like the Raspberry Pi 5 is incredibly inefficient and will consume all your RAM and CPU cores just keeping the model alive. [1, 2, 3]
  • The Solution: Use llama.cpp or Ollama. They use highly-optimized CPU quantization (4-bit or 5-bit GGUF formats). [4, 5, 6]
  • Model Selection: Run Gemma-2-2B-IT (Instruction Tuned) or Gemma-2B-Vision quantized to 4-bits (Q4_K_M). This model will only take up about 1.6 GB to 2.2 GB of your 16GB RAM, leaving plenty of room for your sensors and SLAM algorithms. [5, 6]
  • Expected Speed: You will get roughly 5 to 7 tokens per second. This is more than fast enough for an autonomous vehicle to generate situational reports and make decisions. [2, 4, 7]

2. Camera Video and Vision Processing

  • Do not feed raw video to the LLM: A 2B parameter LLM cannot process 30 frames-per-second video natively on a Pi. [5]
  • The Multi-Tiered Vision Pipeline:
    1. Tier 1 (Real-time): Use a lightweight C++ or Python script running OpenCV or YOLOv8-Nano to scan the live video feed. This will handle immediate object detection (e.g., detecting obstacles, humans, or debris) at 15–20 FPS.
    2. Tier 2 (The LLM Decision): When the vehicle encounters an anomaly, an unknown obstacle, or a target area, the Tier 1 script captures a single high-quality snapshot frame. It passes this image alongside text data to your quantized Gemma Vision model to "analyze the disaster environment" and return a complex text decision. [5, 8, 9, 10, 11]

3. Sensor Fusion & Collision Avoidance

  • GPIO Capabilities: The Pi 5's 40-pin GPIO can easily communicate with your environment sensors (like a DHT22 for heat and humidity) and your collision avoidance systems (such as ultrasonic sensors or a solid-state LiDAR). [12, 13]
  • Architecture Tip: Implement a Robot Operating System (ROS 2 Humble) framework on the Pi. ROS 2 excels at taking data from different sensors, matching their timestamps, and feeding them smoothly to your navigation and AI scripts.

4. Communication: SMS and Offline Fallback

  • The Hardware: Add a GSM/LTE cellular HAT (like the Waveshare SIM7028 NB-IoT HAT or a SIM7600 4G module) directly to the Pi.
  • The Logic Loop: Your Python control script will continuously ping an external server (like 8.8.8.8).
    • If Connected: It utilizes AT commands through the cellular HAT to instantly send out emergency SMS strings or server notifications containing GPS coordinates (if available) or environment summaries.
    • If Offline: It logs all environmental metrics, appends the latest photos/videos into a local database on an external M.2 NVMe SSD, triggers the local Gemma model to adapt its plan, and proceeds with autonomous navigation. [2, 5, 8, 12]

5. No-GPS Autonomous Navigation (SLAM)

  • The Hardware Requirement: Since you cannot use GPS, you must equip the vehicle with a 360-degree 2D/3D LiDAR or a Stereo Depth Camera (like an Intel RealSense), paired with an IMU (Inertial Measurement Unit) sensor. [12]
  • The Software: Use Cartographer SLAM or RTAB-Map SLAM within ROS 2.
  • The Return Strategy (Kidnapped Robot Problem): As the vehicle drives out into the disaster zone, the SLAM algorithm continuously builds a local geometric map of the environment and logs its trajectory coordinates relative to its starting point. When the vehicle decides to "come back," the ROS 2 Navigation stack (Nav2) references this saved internal map to calculate a clean path straight back to (X:0, Y:0, Z:0) without ever needing an external satellite signal.

Suggested Ultimate Hardware Blueprint

To ensure the Pi 5 does not thermal-throttle or bottleneck during this massive operation, configure it with the following hardware stack:
┌────────────────────────────────────────────────────────┐
│               Raspberry Pi 5 (16GB RAM)                │
└───────────────────────────┬────────────────────────────┘
                            │
       ┌────────────────────┼────────────────────┐
       ▼                    ▼                    ▼
 ┌───────────┐        ┌───────────┐        ┌───────────┐
 │ PCIe Slot │        │ USB 3.0   │        │ GPIO Pins │
 └─────┬─────┘        └─────┬─────┘        └─────┬─────┘
       ▼                    ▼                    ▼
┌─────────────┐      ┌─────────────┐      ┌─────────────┐
│ NVMe SSD Base      │ RPLIDAR A1/ │      │ DHT22 Heat/ │
│ (OS & Map   │      │ Depth Cam   │      │ Humid, GSM  │
│ Storage)    │      │ (For SLAM)  │      │ Cellular HAT│
└─────────────┘      └─────────────┘      └─────────────┘
  • Cooling: Use the official Raspberry Pi Active Cooler. The CPU will hit 100% utilization during SLAM and LLM processing, and heat throttling will crash your vehicle.
  • Storage: Do not run this on a MicroSD card. Boot from a high-speed M.2 NVMe SSD using a PCIe HAT. SLAM map writes and video caching will easily corrupt a standard SD card. [5]
This modified pipeline is highly realistic and optimized for a Raspberry Pi 5 (16GB). By cutting out high-bandwidth LiDAR data and dropping the camera feed down to a controlled 20 frames per second (fps) loop with selective frame processing, you save valuable CPU cycles for mapping and AI text analysis. [1]
To solve the "living objects out of sight" requirement (behind walls, dense smoke, or rubble), you can integrate a Far-Infrared Thermal Sensor Array alongside the standard Near-Infrared camera.
The complete blueprint of specific, real-world hardware components and software applications required for this exact disaster recovery vehicle includes:

🛠️ Required Hardware Components & Sensors

1. Central Processing & Power

The main brain running your OS, ROS 2, and local AI model.
Crucial for avoiding thermal throttling under sustained sensor processing. [2, 3]
Essential to power the Pi 5 and the attached array of sensors cleanly via an external battery pack (like a 4S LiPo battery).
For fast local map caching, storing offline logs/photos, and running the operating system reliably instead of using a fragile MicroSD card.

2. Vision & SLAM Sensors (No LiDAR)

  • Arducam OV9281 1MP Global Shutter NoIR Camera (or Raspberry Pi Camera Module 3 NoIR): A "NoIR" (No Infrared Filter) lens is mandatory here. It allows the Pi to read infrared illumination for night-vision/low-light navigation. A Global Shutter camera prevents motion blur during movement, ensuring clear frames for Visual SLAM (V-SLAM) and OpenCV processing. [4, 5, 6, 7]
  • 850nm Infrared LED Illuminator Boards: Mount these next to the camera to flood pitch-black disaster zones with invisible infrared light.

3. Out-of-Sight Life Detection (Thermal Array)

Standard cameras cannot see through dense smoke or darkness to detect heat. This is a 32x24 pixel Far-Infrared sensor array that communicates over I2C. It detects signature human body heat templates (36°C–37°C) even if the individual is obscured by smoke or partially buried out of plain sight. [8, 9]

4. Obstacle & Collision Avoidance

Place these at the front, left, right, and rear of the vehicle. Because you lack LiDAR, these sensors measure physical distance to obstacles. Using I2C or staggered GPIO triggering avoids cross-talk echoes, providing essential distance inputs for the obstacle avoidance layer. [10, 11]
Essential for Visual-Inertial Odometry (VIO). Since you have no LiDAR or GPS, combining the camera tracking with a high-precision accelerometer/gyroscope prevents the vehicle from getting completely lost during V-SLAM mapping.

5. Communication

Provides cellular connectivity to send emergency SMS strings and situational photos to rescue teams whenever network coverage becomes available. [12]

💻 Required Software Stack & Applications

1. Operating System & Frameworks

  • Ubuntu Desktop 24.04 LTS (64-bit): The most robust operating system for running native robotics architectures on the Raspberry Pi 5.
  • ROS 2 Humble Hawksbill: The core communication middleware. It handles node management, orchestrates data from your ultrasonic sensors and IMU, and manages the navigation loop. [13]

2. Vision & Life Detection Software

  • OpenCV (Python or C++ binding): Handles your 20 fps frame buffer pipeline. OpenCV will grab a selective frame, perform pre-processing (grayscale conversions or contrast adjustments), and check for humans or obstacles.
  • YOLOv8-Nano (Ultralytics): Runs natively on the Pi 5's CPU. You can train it specifically to detect human profiles, debris hazards, or missing objects on your selective frames before escalating data to the LLM.
  • Adafruit CircuitPython MLX90640 Library: Pulls raw temperature matrix data over I2C. You can pass this small array into a localized NumPy/SciPy script to detect isolated heat signatures. [12, 13, 14, 15]

3. Navigation & Local Mapping (No-GPS SLAM)

  • ORB-SLAM3 or RTAB-Map (Real-Time Appearance-Based Mapping): Since you do not have LiDAR, you must use a Visual SLAM package. RTAB-Map will intake your NoIR camera frames and your IMU odometry data to build a localized 3D point-cloud environment map, tracking its path relative to its origin point. [16]
  • Nav2 (ROS 2 Navigation Stack): The path-planning application. When the vehicle completes its search, Nav2 references the local map built by RTAB-Map to execute a "Return-to-Home" instruction back to the entry point coordinates (0,0).

4. Edge Local Intelligence

  • Ollama or llama.cpp: The localized runtime execution environment.
  • Gemma-2-2B-IT (GGUF Quantized format): Your core decision-maker model. When OpenCV flags an unknown anomaly or a human heat-signature is found out of sight by the thermal array, the vehicle stops. It structures a text prompt (e.g., "Sensor Alert: Heat 37C found behind smoke stack, ultrasonic distance 1.2m. Determine action."), feeds it to Gemma, and executes the text-based decision returned by the model.

How the Data Flows (The Logic Loop)

[NoIR Camera (20 fps Buffer)] ──► [OpenCV Selective Frame] ──► [YOLOv8-Nano / Human Search]
                                                                        │
[MLX90640 Thermal Sensor Array] ──► [I2C Heat Signature Monitor] ───────┼─► If Triggered ─► [Ollama Gemma 2B] ─► [Nav2 Autonomous Action]
                                                                        │
[Ultrasonic Array + IMU Data] ──► [ROS 2 Sensor Fusion] ──► [RTAB-Map V-SLAM]
Here are the foundational components for your Python-based ROS 2 framework.
Since you are running this entirely offline inside a network-dead zone, we will build a ROS 2 node that reads the MLX90640 Thermal Sensor Array via I2C, tracks your Ultrasonic Array, and marries everything together inside a synchronized architecture.

1. The MLX90640 Thermal I2C Data Parser

This node uses the adafruit-circuitpython-mlx90640 library to extract the 32x24 (768 pixels) temperature matrix via the Pi 5's I2C pins. It calculates the highest temperature found in its field of view. If it detects a value matching human body heat (adjusted for environmental dissipation in a cold or normal tunnel), it publishes an alert. [1, 2] 
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float32, Bool
import board
import busio
import adafruit_mlx90640

class ThermalScannerNode(Node):
    def __init__(self):
        super().__init__('thermal_scanner_node')
        
        # ROS 2 Publishers
        self.temp_pub = self.create_publisher(Float32, 'sensor/thermal/max_temp', 10)
        self.alert_pub = self.create_publisher(Bool, 'sensor/thermal/human_alert', 10)
        
        # Initialize I2C bus on Pi 5 (Default pins: SCL=GPIO3, SDA=GPIO2)
        try:
            self.i2c = busio.I2C(board.SCL, board.SDA, frequency=400000) # 400kHz Fast Mode
            self.mlx = adafruit_mlx90640.MLX90640(self.i2c)
            self.mlx.refresh_rate = adafruit_mlx90640.RefreshRate.REFRESH_4_HZ
            self.get_logger().info('MLX90640 Thermal Array initialized over I2C.')
        except Exception as e:
            self.get_logger().error(f'Failed to initialize Thermal Sensor: {e}')
            return

        # Frame buffer array (768 pixels)
        self.frame = [0] * 768
        
        # Timer loop running at 4 Hz to match sensor refresh rate
        self.timer = self.create_timer(0.25, self.read_thermal_matrix)

    def read_thermal_matrix(self):
        try:
            self.mlx.getFrame(self.frame)
            max_temp = max(self.frame)
            
            # Publish raw maximum temperature
            msg_temp = Float32()
            msg_temp.data = float(max_temp)
            self.temp_pub.publish(msg_temp)
            
            # Human detection window logic (Typical body signature ranges between 30°C to 38°C at a distance)
            msg_alert = Bool()
            if 31.0 <= max_temp <= 39.0:
                msg_alert.data = True
                self.get_logger().warn(f'⚠️ Human heat signature detected out of sight! Max Temp: {max_temp:.2f}°C')
            else:
                msg_alert.data = False
                
            self.alert_pub.publish(msg_alert)
            
        except Exception as e:
            self.get_logger().error(f'Error reading I2C frame: {e}')

def main(args=None):
    rclpy.init(args=args)
    node = ThermalScannerNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2. Central Vision & Ultrasonic Processing Node

This node synchronizes your Ultrasonic distance array (using standard sensor_msgs/msg/Range) with the selective OpenCV frame-skipping logic. It manages the vision processing and serves as the bridge that initiates calls to your local Gemma model when conditions demand an executive decision.
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
from std_msgs.msg import Bool
import cv2
import time

class MasterBrainVisionNode(Node):
    def __init__(self):
        super().__init__('master_brain_vision_node')
        
        # Internal state tracking variables
        self.front_clear_distance = 2.0  # Default safe distance in meters
        self.human_heat_alert = False
        self.frame_count = 0
        self.process_every_n_frames = 10  # Process 1 out of 10 frames (at 20 FPS feed)

        # ROS 2 Subscriptions
        self.sonar_sub = self.create_subscription(Range, 'sensor/ultrasonic/front', self.sonar_callback, 10)
        self.thermal_sub = self.create_subscription(Bool, 'sensor/thermal/human_alert', self.thermal_callback, 10)
        
        # Initialize NoIR Hardware Camera
        self.cap = cv2.VideoCapture(0)
        self.cap.set(cv2.CAP_PROP_FPS, 20)
        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640)
        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
        
        # High-frequency processing timer (Runs at 20Hz loop rate)
        self.timer = self.create_timer(0.05, self.vision_loop)
        self.get_logger().info('Master Vision Node operational at 20 FPS stream baseline.')

    def sonar_callback(self, msg):
        self.front_clear_distance = msg.range

    def thermal_callback(self, msg):
        self.human_heat_alert = msg.data

    def vision_loop(self):
        ret, frame = self.cap.read()
        if not ret:
            return
            
        self.frame_count += 1
        
        # Conditional Execution: Frame-Skipping
        if self.frame_count % self.process_every_n_frames == 0:
            self.execute_edge_analysis(frame)

    def execute_edge_analysis(self, frame):
        # 1. Boost low-light performance of NoIR image
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        equalized = cv2.equalizeHist(gray)
        
        # 2. RUN LOCAL OPENCV DETECTION (Placeholder check)
        visual_human_found = False 
        
        # Decision Matrix evaluation
        if visual_human_found or self.human_heat_alert:
            self.get_logger().fatal('🚨 Human Target verified! Halting vehicle and executing local LLM planning...')
            self.save_and_trigger_llm(frame, raw_reason="Human target located")
            
        elif self.front_clear_distance < 0.4:
            self.get_logger().error('🚨 Imminent structural collision detected by Ultrasonic sensors!')
            self.save_and_trigger_llm(frame, raw_reason="Path blocked by debris obstacle")

    def save_and_trigger_llm(self, frame, raw_reason):
        # Cache snapshot straight to local M.2 NVMe SSD 
        timestamp = int(time.time())
        file_path = f"/mnt/nvme/logs/disaster_snapshot_{timestamp}.jpg"
        cv2.imwrite(file_path, frame)
        
        # This point acts as your handoff to Ollama / Gemma-2B script parameters
        self.get_logger().info(f'[OFFLINE LLM PROMPT GENERATED]: Reason: "{raw_reason}". Image Saved: {file_path}. Navigating response alternative...')

def main(args=None):
    rclpy.init(args=args)
    node = MasterBrainVisionNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.cap.release()
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

3. ROS 2 Launch File Configuration

This ROS 2 Python launch script loads and spins your decoupled nodes simultaneously. It encapsulates them cleanly into a single startup orchestration file.
Create a file named disaster_system_launch.py:
from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        # 1. Thermal Array I2C Node
        Node(
            package='disaster_recovery_system', # Swap with your actual ROS2 workspace package name
            executable='thermal_scanner_node',
            name='thermal_driver',
            output='screen',
            emulate_tty=True
        ),
        
        # 2. Camera Processing and Master Decision Node
        Node(
            package='disaster_recovery_system',
            executable='master_brain_vision_node',
            name='master_brain',
            output='screen',
            emulate_tty=True
        ),
        
        # 3. Placeholder entry for your hardware ultrasonic drivers node
        # Node(
        #     package='disaster_recovery_system',
        #     executable='ultrasonic_sensor_node',
        #     name='sonar_driver',
        #     output='screen'
        # )
    ])
Here is the complete configuration to bring this offline architecture to life. We will add the Ollama subprocess handler directly into your Master Node, configure the hardware wiring pin diagram for your ultrasonic sensors, and set up your ROS 2 deployment package structure.

1. Integrating Ollama & Gemma-2B Into Your Master Node

This update adds a clean, non-blocking Python subprocess pipeline that calls Ollama locally. When an alert occurs, it feeds the sensor data and image metadata to your 4-bit quantized Gemma 2B model and logs its raw textual decision directly onto your M.2 NVMe storage.
Update your master_brain_vision_node.py functions to look like this:
import subprocess
import os

# Insert this new method inside your MasterBrainVisionNode class:
def call_local_gemma_model(self, image_path, reason_prompt):
    self.get_logger().info("🤖 Launching local Gemma-2B inference engine on Pi 5 CPU...")
    
    # Construct a highly detailed prompt optimized for a 2B parameter rescue model
    structured_prompt = (
        f"Context: You are an autonomous disaster recovery vehicle system brain running offline. "
        f"Alert Trigger: {reason_prompt}. "
        f"Evidence Image Saved At: {image_path}. "
        f"System Status: Network dead zone, No GPS, SLAM Tracking Active. "
        f"Task: output exactly one short situational assessment and one immediate navigation directive (e.g., HALT, REVERSE, TURN LEFT, ENGAGE RETURN HOME)."
    )
    
    try:
        # Call Ollama via CLI to save memory allocation overhead vs full python packages
        result = subprocess.run(
            ['ollama', 'run', 'gemma:2b', structured_prompt],
            stdout=subprocess.PIPE,
            stderr=subprocess.PIPE,
            text=True,
            timeout=15.0 # Failsafe constraint to ensure navigation loop never hangs forever
        )
        
        gemma_decision = result.stdout.strip()
        self.get_logger().warn(f"📋 GEMMA DECISION MATRIX RESULT:\n{gemma_decision}")
        
        # Log decision instantly to local NVMe text file for mission debriefing
        log_path = "/mnt/nvme/logs/mission_decisions.txt"
        os.makedirs(os.path.dirname(log_path), exist_ok=True)
        with open(log_path, "a") as log_file:
            log_file.write(f"[{time.strftime('%Y-%m-%d %H:%M:%S')}] Trigger: {reason_prompt}\nDecision: {gemma_decision}\n{'-'*40}\n")
            
    except subprocess.TimeoutExpired:
        self.get_logger().error("❌ Local Gemma inference timed out! Reverting to safety protocol baseline.")
    except Exception as e:
        self.get_logger().error(f"❌ Failed to communicate with Ollama service: {e}")

2. Ultrasonic Sensor Array Hardware Pin Mapping

Because the Pi 5 utilizes the new RP1 I/O controller chip, standard older legacy Python GPIO libraries (like RPi.GPIO) will fail. You must use modern, reliable libraries like gpiod or gpiozero.
Here is how to map a 3-sensor array (Front, Left, Right) using the standard HC-SR04 distance sensors:
        ┌────────────────────────────────────────────────────────┐
        │             Raspberry Pi 5 40-Pin Header               │
        └───────┬────────────────────┬────────────────────┬──────┘
                │                    │                    │
          [Front Sensor]        [Left Sensor]        [Right Sensor]
          ├── Echo: GPIO 23     ├── Echo: GPIO 17    ├── Echo: GPIO 27
          └── Trig: GPIO 24     └── Trig: GPIO 22    └── Trig: GPIO 18
⚠️ CRITICAL VOLTAGE WARNING: The HC-SR04 sensor outputs a 5V Echo signal, but the Raspberry Pi 5 pins only accept 3.3V max. You must use a simple resistor voltage divider (e.g., a 1kΩ and a 2kΩ resistor) on each Echo wire before plugging it into the Pi, otherwise you will permanently burn out your Pi 5's GPIO pins.

3. ROS 2 Package Infrastructure Configuration

To make ROS 2 compile your new Python setup properly on the Pi, configure your workspace files exactly as follows:

package.xml

Place this in your workspace directory (~/ros2_ws/src/disaster_recovery_system/package.xml):
<?xml version="1.0"?>
<?xml-model href="http://ros.org" schematypens="http://w3.org"?>
<package format3>
  <name>disaster_recovery_system</name>
  <version>1.0.0</version>
  <description>Offline Autonomous Disaster Vehicle Controller Stack</description>
  <maintainer email="vehicle@edge.local">Operator</maintainer>
  <license>Apache-2.0</license>

  <depend>rclpy</depend>
  <depend>std_msgs</depend>
  <depend>sensor_msgs</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
  <test_depend>ament_pep257</test_depend>
  <test_depend>python3-pytest</test_depend>

  <export>
    <build_type>ament_python</build_type>
  </export>
</package>

setup.py

Place this in your workspace directory (~/ros2_ws/src/disaster_recovery_system/setup.py):
from setuptools import find_packages, setup
import os
from glob import glob

package_name = 'disaster_recovery_system'

setup(
    name=package_name,
    version='1.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        # Include your Python launch files
        (os.path.join('share', package_name, 'launch'), glob('launch/*launch.[pxy][yma]*')),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='Operator',
    maintainer_email='vehicle@edge.local',
    description='Offline Autonomous Disaster Vehicle Controller Stack',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'thermal_scanner_node = disaster_recovery_system.thermal_scanner_node:main',
            'master_brain_vision_node = disaster_recovery_system.master_brain_vision_node:main',
        ],
    },
)

4. Step-by-Step Installation Commands on the Pi 5

Run these commands inside your terminal to install the dependencies and pull down your offline AI model:
# 1. System updates and dependencies install
sudo apt update && sudo apt install -p python3-pip python3-opencv -y

# 2. Install Adafruit CircuitPython library stack for I2C Thermal Reading
pip3 install adafruit-circuitpython-mlx90640 adafruit-blinka

# 3. Download and test Ollama natively on the board
curl -fsSL https://ollama.com | sh
ollama run gemma:2b "Test communication."

# 4. Build your ROS 2 Workspace
cd ~/ros2_ws
colcon build --packages-select disaster_recovery_system
source install/setup.bash

# 5. Launch the entire integrated project!
ros2 launch disaster_recovery_system disaster_system_launch.py
Yes, we absolutely need to write that specialized driver node. Because the Raspberry Pi 5 completely separates its GPIO management from the main CPU using the RP1 I/O controller chip, standard legacy scripts will throw errors. Using the modern gpiozero library ensures accurate timing for the ultrasonic Echo pulses without crashing your ROS 2 threads. [1]
Here is the complete Ultrasonic Array Node designed to read three distance sensors simultaneously, safely manage their timing loop to avoid echo cross-talk, and publish the results directly to the master brain node.

1. The ROS 2 Python Ultrasonic Driver Node

Create a new file named ultrasonic_sensor_node.py inside your package directory (~/ros2_ws/src/disaster_recovery_system/disaster_recovery_system/):
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
from gpiozero import DistanceSensor
import time

class UltrasonicArrayDriver(Node):
    def __init__(self):
        super().__init__('ultrasonic_array_driver')
        
        # ROS 2 Range Publishers
        self.front_pub = self.create_publisher(Range, 'sensor/ultrasonic/front', 10)
        self.left_pub = self.create_publisher(Range, 'sensor/ultrasonic/left', 10)
        self.right_pub = self.create_publisher(Range, 'sensor/ultrasonic/right', 10)
        
        # Initialize gpiozero Distance Sensors (Pi 5 Hardware Compatible)
        # Max distance bound set to 3.0 meters for rapid sampling in tight disaster spaces
        try:
            self.sensor_front = DistanceSensor(echo=23, trigger=24, max_distance=3.0)
            self.sensor_left  = DistanceSensor(echo=17, trigger=22, max_distance=3.0)
            self.sensor_right = DistanceSensor(echo=27, trigger=18, max_distance=3.0)
            self.get_logger().info('✅ All 3 Ultrasonic sensors initialized successfully on Pi 5 RP1 Pins.')
        except Exception as e:
            self.get_logger().error(f'❌ GPIO Initialization Failed: {e}')
            return

        # Sequential polling state tracking
        self.sensor_sequence = ['FRONT', 'LEFT', 'RIGHT']
        self.current_index = 0

        # High-frequency scanning timer (Runs at 30Hz loop speed)
        # Every tick polls exactly one sensor to prevent cross-talk pulse overlapping
        self.timer = self.create_timer(0.033, self.sequential_scan_loop)

    def sequential_scan_loop(self):
        target_sensor = self.sensor_sequence[self.current_index]
        
        # Build standard ROS 2 structural metadata payload
        range_msg = Range()
        range_msg.header.stamp = self.get_clock().now().to_msg()
        range_msg.radiation_type = Range.ULTRASONIC
        range_msg.field_of_view = 0.26  # Roughly 15 degrees tracking arc
        range_msg.min_range = 0.02      # 2 cm minimum limit
        range_msg.max_range = 3.0      # 3 meters maximum limit

        try:
            if target_sensor == 'FRONT':
                range_msg.header.frame_id = 'ultrasonic_front_link'
                range_msg.range = float(self.sensor_front.distance)
                self.front_pub.publish(range_msg)
                
            elif target_sensor == 'LEFT':
                range_msg.header.frame_id = 'ultrasonic_left_link'
                range_msg.range = float(self.sensor_left.distance)
                self.left_pub.publish(range_msg)
                
            elif target_sensor == 'RIGHT':
                range_msg.header.frame_id = 'ultrasonic_right_link'
                range_msg.range = float(self.sensor_right.distance)
                self.right_pub.publish(range_msg)
                
        except Exception as e:
            self.get_logger().error(f'Error reading sensor {target_sensor}: {e}')

        # Move execution pointer to next hardware sensor in array sequence
        self.current_index = (self.current_index + 1) % len(self.sensor_sequence)

def main(args=None):
    rclpy.init(args=args)
    node = UltrasonicArrayDriver()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

2. Updating Your Setup Manifests

To make sure ROS 2 builds this new hardware script, you must add the new entry point to your existing package files.

Update setup.py Entry Points:

Open your setup.py and modify the entry_points block to include the sonar node:
    entry_points={
        'console_scripts': [
            'thermal_scanner_node = disaster_recovery_system.thermal_scanner_node:main',
            'master_brain_vision_node = disaster_recovery_system.master_brain_vision_node:main',
            'ultrasonic_sensor_node = disaster_recovery_system.ultrasonic_sensor_node:main', # <-- ADD THIS LINE
        ],
    },

Update disaster_system_launch.py:

Uncomment the placeholder in your launch file so that it kicks off all three scripts together automatically:
        # 3. Hardware ultrasonic drivers node
        Node(
            package='disaster_recovery_system',
            executable='ultrasonic_sensor_node',
            name='sonar_driver',
            output='screen',
            emulate_tty=True
        )

3. Missing System Package Dependencies

Before recompiling your workspace, you must make sure the modern Pi 5 GPIO backend packages are present on your operating system. Run this in your terminal:
# Install the missing system and user space libraries
sudo apt update
sudo apt install python3-gpiozero python3-gpiod -y

# Recompile the workspace on your Pi 5
cd ~/ros2_ws
colcon build --packages-select disaster_recovery_system
source install/setup.bash

-
Labels: , , , ,
Subscribe to: Posts (Atom)

House Based Manufacturing Micro Clustering

                                 image generated by meta ai House-based manufacturing micro-clustering in China refers to the hyper-local, v...