# Project export: HeartStart

This document was generated by HackStack to give an AI agent context about a hackathon project. Sections are labeled with their provenance; content marked as truncated was cut to keep this document small.

## Project metadata

- Hackathon: TreeHacks 2026
- Tagline: Emergency Detection. Autonomous Robot CPR.
- Devpost: https://devpost.com/software/heartstart
- GitHub: https://github.com/nicolewongbiz/cpr-robot.git
- Video: https://www.youtube.com/embed/8B98pMV5k1o?enablejsapi=1&hl=en_US&rel=0&start=&version=3&wmode=transparent
- Result: winner (Best Beginner Hack)
- Team: 3 GitHub contributor(s) — nicolewongbiz (4 commits), chilinghan (3 commits), Cris (1 commits)

## Devpost submission (written by the team)

### Inspiration

Every year, more than 350,000 people in the US experience cardiac arrest outside of a hospital setting. The difference between life and death often comes down to those critical minutes before emergency services arrive—minutes where CPR must be performed continuously, consistently, and correctly. But bystanders freeze, panic, or simply don't know how to administer CPR properly. First responders face impossible response times. And even trained individuals grow fatigued, their compressions becoming shallow and ineffective after just two minutes. In fact, according to the American Heart Association, the survival rate drops by 7–10% for every minute without CPR. After just 10 minutes, survival is nearly zero. Yet the average EMS response time in the United States is over 7 minutes in urban areas—and much longer in rural communities. We asked ourselves: what if a robot could be there instantly? What if a device never tires, never panics, and never forgets the rhythm? What if we could build a guardian that watches over loved ones and acts the moment their heart stops? That's why we built HeartStart—a name that captures both the urgency of cardiac response and the promise of a new beginning. An autonomous CPR robot that monitors a patient's heart, detects cardiac arrest within seconds, navigates to their location, and delivers consistent, high-quality chest compressions until help arrives. Immediate CPR can double or triple the chance of survival, and that is the impact we hope to deliver to society.

### What it does

HeartStart is an autonomous mobile robot that continuously monitors a patient's heart rate via a wearable heart-rate sensor. When cardiac arrest occurs, HeartStart springs into action: 1) Immediate Alert: The system triggers an API call via Twilio, notifying emergency services with a custom message: "Cardiac arrest detected at [location]. Autonomous CPR robot is on-site and administering aid." (For our demo, we routed this to a friend's phone rather than actual 911.) 2) Autonomous Navigation: The robot navigates toward the patient using computer vision, tracking their body position and using depth sensing to approach safely. Once within range, it locks onto an AprilTag placed on the patient's chest for precise positioning. 3) Precision CPR: After positioning itself directly above the chest, HeartStart deploys its CPR mechanism to deliver consistent, high-quality chest compressions at the correct depth and rate—never tiring, never losing rhythm. 4) Real-Time Monitoring: Throughout the process, the robot's onboard display shows the patient's heart rate, IR signal, and medical history, keeping any human responders informed the moment they arrive.

### How we built it

The Sensing System We used a sensor worn by the patient to continuously monitor heart activity. For our hackathon demo, we connected this sensor to an Arduino, which fed data via serial monitor to a laptop. Using PySerial, we streamed this data in real-time to a web application that displays the patient's heart rate and IR signal. Our ultimate aim is full wireless real-time data transfer, but this wired solution gave us the reliability needed for a working prototype. The Vision System The robot navigates using Arducam cameras and a depth-based computer vision pipeline built with ROS2. The process has two phases: Approach Phase: The robot tracks the patient's full body, maintaining a frame of reference to navigate toward them safely. By projecting the subject's bounding box onto the camera's image plane, we mapped their position to directional commands sent to the Raspberry Pi. The robot's speed was set proportional to log(r), where r is the ratio of bounding box area to total frame area, allowing faster approach when the subject was far away and gradual slowing as the robot drew near. Positioning Phase: Once closer, the robot detects an AprilTag placed on the patient's chest and switches to fine-tuned positioning, stopping precisely when correctly aligned above the sternum. The Control System Data flows from the laptop (running the monitoring and vision processing) to the Raspberry Pi onboard the robot. When the flatline trigger activates, the Pi receives the command and initiates navigation. After positioning is confirmed, the Pi activates the CPR mechanism. The User Interface We built a clean, informative web app using HTML and Python that displays: Real-time heart rate and IR signal Patient health history (accessible to responders) A custom 3D heart animation designed in Blender, which pulses with each detected heartbeat—making the interface both informative and approachable. The CPR Mechanism We manipulated cranker shaft to create linear motion from rotational motion in a highly cost-efficient way. 3D printed using PLA.

### Challenges we ran into

Single-Camera Depth Perception Our most technically challenging problem was extracting depth information from a single Arducam. Without stereo vision or a LIDAR sensor, we had to estimate distance using only one camera. Getting the robot to accurately judge how far away the patient was proved unsurprisingly difficult. We experimented with ground plane estimation and size-based distance calculations, but each approach had limitations. In the end, we decided to treat the subject's location box as a projection of the frame of reference for the middle of the camera, and then use these mappings to send directions to the Raspberry Pi. The AprilTag solution worked well for final positioning, but first we needed to get within range to detect it. Calibrating the camera for reliable measurements took many iterations. System Integration The biggest challenge was getting all the components to work together reliably. The Arduino communicated with the laptop over serial, the laptop ran Python scripts to update the web interface and send commands to the Raspberry Pi, and the Pi ran ROS2 nodes for vision processing and motor control. Every part needed to stay synchronized. When something failed, debugging meant tracing through multiple systems written in different languages. We learned to implement comprehensive logging at every stage and to test components both individually and as a complete system. There were many late nights spent tracking down why the vision system would work perfectly in isolation but fail when connected to the motor controllers. Hardware Hardware was hard... especially with handling some soldered joints...

### Accomplishments we're proud of

We're incredibly proud of the technical foundation we built for HeartStart. On the sensing side, we integrated a BLE wearable sensor with an Arduino, streaming real-time heart rate data over serial. Using PySerial, we piped that data into a Python backend that continuously monitored for flatline conditions. The moment the signal crossed the 15-second threshold of flat line, our system triggered an automated Twilio API call, closing the loop from physiological event to emergency notification. For the web interface, we built a clean, informative dashboard using HTML and Python. It displays live heart rate, IR signal, and patient health history, all updating in real-time as data streams in. But the highlight is the custom 3D heart animation we designed in Blender, which pulses with each detected heartbeat. It turns a clinical monitoring screen into something more approachable and human. We built the entire navigation system on ROS2, using Arducams for vision and depth estimation. Getting the robot to track a person, approach them safely, and then lock onto an AprilTag for precise positioning required integrating computer vision, coordinate transforms, and motor control into a single coherent pipeline. The two-stage approach—full body tracking for approach, AprilTag for fine positioning—was something we figured out through experimentation. On the communication side, we established a reliable data pipeline from the laptop (running monitoring and vision) to the Raspberry Pi (controlling the robot). When the flatline trigger activates, commands flow through this pipeline to start navigation, and later to deploy the CPR mechanism. What makes us proudest is not a single component, but that the system flows smoothly. The Arduino talks to Python, Python talks to the web interface and the Pi, the Pi runs ROS2 nodes that process camera data and drive motors, and every piece stays synchronized enough to respond to a life-threatening event in real-time. Building a fully functional autonomous system feels like a genuine accomplishment.

### What we learned

This project taught us the enormous gap between a concept and a working physical system. Software can be debugged with print statements; hardware requires patience, calibration, and often a complete rethink of assumptions. We also deepened our skills in ROS2, computer vision, and embedded systems. The biggest lesson was ultimately about impact. Medical technology isn't just about clever algorithms; it's about reliability, safety, and trust. A CPR robot that fails is not just a bug, but a life that could have been saved. That responsibility shaped every decision we made.

### What's next

Multi-Room Navigation Currently, HeartStart assumes the patient is in the same room. Our next major goal is enabling true multi-room navigation using SLAM (Simultaneous Localization and Mapping), allowing the robot to monitor and respond to a patient anywhere in the home, like through doorways, around furniture, and across different spaces. Continuous Patient Tracking Rather than waiting for a flatline to locate the patient, we want HeartStart to maintain awareness of the patient's position at all times. This means integrating additional BLE beacons for coarse location tracking and using the robot's cameras to periodically update and remember where the patient is throughout the day. Enhanced Medical Capabilities CPR is just the beginning. We envision expanding HeartStart into a more comprehensive emergency response platform with integrated AED defibrillation, oxygen delivery, and additional vital sign monitoring like blood oxygen and respiratory rate, whilst maintaining the same autonomous response capabilities. Integration with Emergency Systems HeartStart could also work with smart home systems to trigger lights and unlock doors for arriving EMS, whilst also building telemedicine capabilities that give dispatchers and first responders real-time patient data, video feeds, and a complete log of events before they even arrive.

## README (from the GitHub repository)

No README available.

## Detected evidence (automated analysis)

Indexed codebase: 31 recognized source files, 60 KB.
- HTML (language) — detected in the code
- Python (language) — detected in the code
- C (language) — claimed on Devpost, not found in the code
- CSS (language) — claimed on Devpost, not found in the code
- JavaScript (language) — claimed on Devpost, not found in the code

## Codebase structure (from repository index)

### Files (47 of 47)

```
.gitattributes
.gitignore
communicateWithPi/communicateWithPi.ino
cprobot/Dockerfile
cprobot/scripts/build.sh
cprobot/scripts/docker.sh
cprobot/scripts/release_cameras.sh
cprobot/scripts/yolo_udp_sender.py
cprobot/src/arduino_bridge/arduino_bridge/__init__.py
cprobot/src/arduino_bridge/arduino_bridge/bridge_node.py
cprobot/src/arduino_bridge/package.xml
cprobot/src/arduino_bridge/resource/arduino_bridge
cprobot/src/arduino_bridge/setup.cfg
cprobot/src/arduino_bridge/setup.py
cprobot/src/arduino_bridge/test/test_copyright.py
cprobot/src/arduino_bridge/test/test_flake8.py
cprobot/src/arduino_bridge/test/test_pep257.py
cprobot/src/control/control/__init__.py
cprobot/src/control/control/controller.py
cprobot/src/control/launch/control.launch.py
cprobot/src/control/package.xml
cprobot/src/control/resource/control
cprobot/src/control/setup.cfg
cprobot/src/control/setup.py
cprobot/src/control/test/test_copyright.py
cprobot/src/control/test/test_flake8.py
cprobot/src/control/test/test_pep257.py
cprobot/src/perception/launch/perception_udp.launch.py
cprobot/src/perception/package.xml
cprobot/src/perception/perception/__init__.py
cprobot/src/perception/perception/apriltag_udp.py
cprobot/src/perception/perception/cmd_vel_arbiter.py
cprobot/src/perception/perception/person_follower_udp.py
cprobot/src/perception/resource/perception
cprobot/src/perception/setup.cfg
cprobot/src/perception/setup.py
cprobot/src/perception/test/test_copyright.py
cprobot/src/perception/test/test_flake8.py
cprobot/src/perception/test/test_pep257.py
healthApp/index.html
healthApp/models/heart.glb
healthApp/server.py
healthSensor/health_sensor.ino
heart_data
pump/pump.ino
testingAllMotors/testingAllMotors.ino
testingServos/testingServos.ino
```

### Dependencies

No dependency index available.

### Recent commits (newest first)

- Add all arduino code
- [Fix] error in perception launch file
- ros2 nodes for control and perception stack
- heart monitor to twilio api
- real time data from arduino
- communicate w arduino
- updated UI
- Add heart rate monitoring functionality
- Add project files with LFS
- Configure Git LFS
- [Add] live heart
- cursor
- updated
- UI
- Add files via upload

## Key source files (fetched from GitHub, selected and truncated for size)

### cprobot/Dockerfile

```
FROM ros:jazzy-ros-base-noble

SHELL ["/bin/bash", "-c"]

RUN apt-get update && apt-get install -y \
    python3-serial \
    python3-pip \
    python3-dev \
    build-essential \
    libgl1 \
    libglib2.0-0 \
    python3-wheel \
    python3-colcon-common-extensions

RUN echo "source /opt/ros/jazzy/setup.bash" >> /etc/bash.bashrc
WORKDIR /ws

CMD ["bash"]

```

### healthApp/server.py

```python
import serial
import socketio
import eventlet
import time
import os
from flask import Flask, send_from_directory
from dotenv import load_dotenv
from twilio.rest import Client

# Load variables from .env file
load_dotenv()

sio = socketio.Server(cors_allowed_origins='*')
app = Flask(__name__)

# --- CONFIGURATION ---
SERIAL_PORT = os.getenv('SERIAL_PORT', '/dev/cu.usbmodem1101') 
BAUD_RATE = 115200

# Twilio Setup
TWILIO_SID = os.getenv('TWILIO_ACCOUNT_SID')
TWILIO_TOKEN = os.getenv('TWILIO_AUTH_TOKEN')
TWILIO_FROM = os.getenv('TWILIO_PHONE_NUMBER')
TWILIO_TO = os.getenv('TARGET_PHONE_NUMBER')

try:
    ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1)
    time.sleep(2)
    ser.reset_input_buffer()
    print(f"✅ Arduino connected on {SERIAL_PORT}")
except Exception as e:
    ser = None
    print(f"⚠️ Serial Connection Error: {e}")

# --- TWILIO VOICE HELPER ---
def make_emergency_call(bpm_value):
    """Triggers a phone call with a text-to-speech message."""
    try:
        if all([TWILIO_SID, TWILIO_TOKEN, TWILIO_FROM, TWILIO_TO]):
            client = Client(TWILIO_SID, TWILIO_TOKEN)
            
            # TwiML defines what the robot says when you answer
            twiml_content = f'''
            <Response>
                <Say voice="alice">
                    Attention. This is an automated emergency alert from your heart monitor. 
                    A flatline has been detected for fifteen seconds. 
                    The last recorded heart rate was {bpm_value} beats per minute. 
                    Please check the patient immediately.
                </Say>
            </Response>
            '''
            
            call = client.calls.create(
                twiml=twiml_content,
                from_=TWILIO_FROM,
                to=TWILIO_TO
            )
            print(f"📞 Twilio Call Placed: {call.sid}")
        else:
            print("⚠️ Twilio credentials missing in .env")
    except Exception as e:
        print(f"❌ Twilio Call Failed: {e}")

# --- ROUTES ---
@app.route('/')
def index():
    return send_from_directory('.', 'index.html')

@app.route('/models/<path:filename>')
def serve_models(filename):
    return send_from_directory('models', filename)

def read_arduino():
    print("Loop started: Waiting for Arduino data...")
    zero_hr_start_time = None
    emergency_triggered = False

    while True:
        if ser and ser.is_open:
            try:
                raw_data = ser.readline()
                if raw_data:
                    line = raw_data.decode('utf-8', errors='ignore').strip()
                    if "," in line:
                        parts = line.split(',')
                        if len(parts) >= 3:
                            hr = int(parts[0])
                            ir = int(parts[1])
                            beat = int(parts[2])
                            
                            # Local Dashboard Update
                            sio.emit('update_data', {'hr': hr, 'ir': ir, 'beat': beat})

                            # --- EMERGENCY LOGIC ---
                            if hr == 0:
                                if zero_hr_start_time is None:
                                    zero_hr_start_time = time.time()
                                
                                elapsed = time.time() - zero_hr_start_time
                                if elapsed >= 15 and not emergency_triggered:
                                    print("🚨 ALERT: 15s of 0 BPM. Calling Emergency Contact...")
                                    sio.emit('emergency_alert', {'active': True})
                                    emergency_triggered = True
                                    # Trigger the Voice Call
                                    make_emergency_call(hr)
                            else:
                                # Heartbeat detected - reset everything
                                if emergency_triggered:
                                    sio.emit('emergency_alert', {'active': False})
                                    emergency_triggered = False
                                zero_hr_start_time = None

            except Exception as e:
                print(f"Read Error: {e}")
        eventlet.sleep(0.01)

if __name__ == '__main__':
    app.wsgi_app = socketio.WSGIApp(sio, app.wsgi_app)
    eventlet.spawn(read_arduino)
    print("🚀 Server starting at http://localhost:5000")
    eventlet.wsgi.server(eventlet.listen(('0.0.0.0', 5000)), app)
```

### healthApp/index.html

```html
<!DOCTYPE html>
<html lang="en">
<head>
    <meta charset="UTF-8">
    <meta name="viewport" content="width=device-width, initial-scale=1.0">
    <title>BioStat | Premium</title>
    
    <script src="https://cdnjs.cloudflare.com/ajax/libs/socket.io/4.0.1/socket.io.js"></script>
    <script src="https://cdnjs.cloudflare.com/ajax/libs/smoothie/1.34.0/smoothie.min.js"></script>
    <link href="https://fonts.googleapis.com/css2?family=Inter:wght@400;700;900&display=swap" rel="stylesheet">

    <style>
        body {
            margin: 0; font-family: 'Inter', sans-serif;
            background: radial-gradient(at 0% 0%, #ffdee9 0%, transparent 50%),
                        radial-gradient(at 100% 0%, #b5fffc 0%, transparent 50%);
            background-color: #f5f5f7;
            display: flex; flex-direction: column; align-items: center; padding: 20px; min-height: 100vh;
            overflow-x: hidden;
        }
        .container { width: 100%; max-width: 360px; }
        
        /* Glass UI */
        .card, .model-container {
            background: rgba(255, 255, 255, 0.2);
            backdrop-filter: blur(25px) saturate(180%);
            border-radius: 24px; margin-bottom: 20px;
            border: 1px solid rgba(255, 255, 255, 0.4);
            box-shadow: 0 10px 30px rgba(0,0,0,0.05);
            overflow: hidden; position: relative;
        }
        .model-container { height: 320px; cursor: grab; }
        .model-container:active { cursor: grabbing; }
        .card { padding: 24px; }
        
        #canvas-container { width: 100%; height: 100%; }
        .label { font-size: 12px; font-weight: 700; color: #6a737d; text-transform: uppercase; }
        .value-text { font-size: 48px; font-weight: 900; margin: 0; transition: transform 0.1s; display: inline-block;}
        #hr-val { color: #d73a49; }
        
        canvas.data-chart { width: 100% !important; height: 80px !important; margin-top: 10px; background: transparent; }
        #status { font-weight: 700; margin-bottom: 15px; padding: 8px 20px; background: rgba(255, 255, 255, 0.5); border-radius: 50px; transition: all 0.3s; }

        /* Emergency Alert Banner */
        #emergency-alert {
            position: fixed; top: 80px; left: 50%; transform: translateX(-50%);
            background: #d73a49; color: white; padding: 12px 24px; border-radius: 50px;
            font-weight: 900; letter-spacing: 1px; box-shadow: 0 10px 20px rgba(215, 58, 73, 0.4);
            display: none; z-index: 2000; animation: pulse-red 1.5s infinite;
        }
        @keyframes pulse-red {
            0% { transform: translateX(-50%) scale(1); }
            50% { transform: translateX(-50%) scale(1.05); }
            100% { transform: translateX(-50%) scale(1); }
        }

        /* Sidebar Panel */
        #patient-sidebar {
            position: fixed; top: 0; right: -350px; width: 300px; height: 100vh;
            background: rgba(255, 255, 255, 0.8); backdrop-filter: blur(30px);
            z-index: 1000; padding: 30px; transition: right 0.4s cubic-bezier(0.16, 1, 0.3, 1);
            border-left: 1px solid rgba(255, 255, 255, 0.3); box-shadow: -10px 0 30px rgba(0,0,0,0.05);
        }
        #patient-sidebar.open { right: 0; }
        .toggle-btn {
            position: fixed; top: 20px; right: 20px; z-index: 1001;
            background: white; border: none; padding: 10px 15px; border-radius: 12px;
            cursor: pointer; font-weight: 700; box-shadow: 0 4px 10px rgba(0,0,0,0.1);
        }
        .record-item { margin-bottom: 20px; border-bottom: 1px solid rgba(0,0,0,0.05); padding-bottom: 10px; }
        .record-item label { font-size: 10px; color: #666; text-transform: uppercase; }
        .record-item div { font-weight: 700; font-size: 14px; }
    </style>
</head>
<body>

    <button class="toggle-btn" onclick="document.getElementById('patient-sidebar').classList.toggle('open')">☰ PATIENT</button>
    <div id="emergency-alert">🚨 911 CONTACTED - EMERGENCY SERVICES NOTIFIED</div>

    <div id="patient-sidebar">
        <h2 style="margin-top: 40px;">John Doe</h2>
        <p style="color: #666; font-size: 12px; margin-bottom: 30px;">ID: #88-X-992</p>
        <div class="record-item"><label>Condition</label><div>Sinus Tachycardia</div></div>
        <div class="record-item"><label>Medication</label><div>Atenolol 25mg</div></div>
        <div class="record-item"><label>Last Reading</label><div>Stable - 14 Feb 2026</div></div>
        <div class="record-item"><label>Blood Type</label><div>O Positive</div></div>
    </div>

    <div id="status">INITIALIZING...</div>

    <div class="container">
        <div class="model-container">
            <div id="canvas-container"></div>
        </div>

        <div class="card">
            <div class="label">Heart Rate (BPM)</div>
            <div id="hr-val" class="value-text">--</div>
            <canvas id="hr-chart" class="data-chart"></canvas>
        </div>

        <div class="card">
            <div class="label">Real-time Pulse Wave (IR)</div>
            <canvas id="ir-chart" class="data-chart"></canvas>
        </div>
    </div>

    <script type="importmap">
    {
        "imports": {
            "three": "https://cdn.jsdelivr.net/npm/three@0.160.0/build/three.module.js",
            "three/addons/": "https://cdn.jsdelivr.net/npm/three@0.160.0/examples/jsm/"
        }
    }
    </script>

    <script type="module">
        import * as THREE from 'three';
        import { GLTFLoader } from 'three/addons/loaders/GLTFLoader.js';
        import { OrbitControls } from 'three/addons/controls/OrbitControls.js';

        const socket = io();
        const hrSeries = new TimeSeries();
        const irSeries = new TimeSeries();

        let scene, camera, renderer, model, mixer, controls;
        let clock = new THREE.Clock();
        let baseScale = 1.0, currentScale = 1.0, targetScale = 1.0;

        function init() {
            const container = document.getElementById('canvas-container'
[truncated — 5176 more characters]
```

### cprobot/scripts/build.sh

```shell
#!/usr/bin/env bash
set -e

colcon build --symlink-install

echo ""
echo "To use the build:  source $(pwd)/install/setup.bash"

```

### cprobot/scripts/docker.sh

```shell
#!/usr/bin/env bash
# bash scripts/docker.sh
set -e

IMAGE_NAME="cprobot"
HOST_WS="${HOST_WS:-$HOME/projects/cprobot}"
CONTAINER_WS="/ws"

docker run -it --rm \
  --net=host \
  -v "${HOST_WS}:${CONTAINER_WS}" \
  -w "${CONTAINER_WS}" \
  -e HOST_UID="$(id -u)" \
  -e HOST_GID="$(id -g)" \
  "${IMAGE_NAME}" \
  bash

```

### cprobot/scripts/release_cameras.sh

```shell
#!/usr/bin/env bash
# Kill any leftover run.sh camera streams so rpicam-app / rpicam-hello can acquire the camera.
# Run this on the HOST (not inside the container). If you get "failed to acquire camera", run: bash scripts/release_cameras.sh

set -euo pipefail

do_kill() {
  local sudo="$1"
  # 1) Kill what's on the stream ports (ffmpeg listeners)
  $sudo fuser -k 5000/tcp 5001/tcp 2>/dev/null || true
  # 2) Kill the whole process group for each rpicam-vid (stops the "while true" loop that restarts it)
  for pid in $(pgrep -x rpicam-vid 2>/dev/null || true); do
    [[ -z "$pid" ]] && continue
    pgid=$(ps -o pgid= -p "$pid" 2>/dev/null | tr -d ' ')
    [[ -n "$pgid" ]] && $sudo kill -TERM -"$pgid" 2>/dev/null || true
  done
  $sudo pkill -x rpicam-vid 2>/dev/null || true
  $sudo pkill -f flask_stream.py 2>/dev/null || true
}

echo "[release_cameras] Stopping processes using cameras / stream ports (run on host, not in container)..."

do_kill ""

sleep 1
if pgrep -x rpicam-vid >/dev/null 2>&1; then
  echo "[release_cameras] Still running — trying with sudo (streams may have been started as root)..."
  do_kill "sudo"
  sleep 1
fi

if pgrep -x rpicam-vid >/dev/null 2>&1; then
  echo "[release_cameras] rpicam-vid still running. Try: sudo bash scripts/release_cameras.sh"
  exit 1
fi

echo "[release_cameras] Done. Try rpicam-app or rpicam-hello now."

```

### cprobot/scripts/yolo_udp_sender.py

```python
#!/usr/bin/env python3
"""
Run YOLO on cam0 and AprilTag on cam1; send detections over UDP for ROS2 nodes.
  cam0 (YOLO only)  → 127.0.0.1:5000  (person: cx, area, conf, t)
  cam1 (AprilTag only) → 127.0.0.1:5010  (tag_id, margin, x, y, z, yaw, t)

Backends: picamera2 (system Python + python3-libcamera) or opencv (venv).
Requires: ultralytics, opencv-python, pupil-apriltags. Optional: picamera2.
"""
import argparse
import os
import socket
import sys
import time

import numpy as np

from picamera2 import Picamera2
from ultralytics import YOLO
import cv2
from pupil_apriltags import Detector as AprilTagDetector


def run_yolo_and_draw(frame, model, conf_thresh, imgsz):
    """Run YOLO, draw person boxes on a copy. Return (vis_frame, cx_norm, area_norm, conf)."""
    H, W = frame.shape[:2]
    results = model.predict(frame, imgsz=imgsz, conf=conf_thresh, verbose=False)
    boxes = results[0].boxes
    vis = frame.copy()
    best = None
    for b in boxes:
        if int(b.cls[0]) != 0:
            continue
        x1, y1, x2, y2 = map(int, b.xyxy[0])
        cv2.rectangle(vis, (x1, y1), (x2, y2), (0, 255, 0), 2)
        cv2.putText(vis, "person", (x1, y1 - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
        area = max(1.0, (x2 - x1) * (y2 - y1))
        cx = (x1 + x2) / 2.0
        cx_norm = (cx - W / 2.0) / (W / 2.0)
        area_norm = min(1.0, area / float(W * H))
        c = float(b.conf[0])
        if best is None or area > best[0]:
            best = (area, cx_norm, area_norm, c)
    if best is None:
        return (vis, None, None, None)
    return (vis, best[1], best[2], best[3])


def run_tag_and_draw(frame, detector, tag_size, fx, fy, cx, cy, tag_id_filter):
    """Run AprilTag, draw tag corners on a copy. Return (vis_frame, tag_id, margin, x, y, z, yaw)."""
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    tags = detector.detect(
        gray,
        estimate_tag_pose=True,
        camera_params=(fx, fy, cx, cy),
        tag_size=tag_size,
    )
    vis = frame.copy()
    best = None
    for t in tags:
        pts = np.array(t.corners, dtype=np.int32)
        cv2.polylines(vis, [pts], True, (0, 255, 255), 2)
        c = pts.mean(axis=0).astype(int)
        cv2.putText(vis, str(t.tag_id), tuple(c), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 255), 2)
        if tag_id_filter != -1 and t.tag_id != tag_id_filter:
            continue
        if best is None or t.decision_margin > best.decision_margin:
            best = t
    if best is None:
        return (vis, None, None, None, None, None, None)
    x, y, z = best.pose_t.flatten().tolist()
    R = best.pose_R
    yaw = float(np.arctan2(R[1, 0], R[0, 0]))
    return (vis, best.tag_id, best.decision_margin, x, y, z, yaw)


# ---------- camera backends ----------
def _make_picam(index, width, height):
    try:
        picam = Picamera2(index)
        config = picam.create_preview_configuration(main={"size": (width, height)})
        picam.configure(config)
        picam.start()
        return picam, None
    except Exception as e:
        return None, e


def _read_picam(picam, to_bgr):
    frame = picam.capture_array()
    if to_bgr and frame is not None:
        frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)
    return frame


def _make_cv2(index, width, height):
    cap = cv2.VideoCapture(index)
    if not cap.isOpened():
        return None, RuntimeError(f"Cannot open camera {index}")
    cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
    return cap, None


def _read_cv2(cap):
    ok, frame = cap.read()
    return frame if ok else None


def main():
    p = argparse.ArgumentParser(description="cam0=YOLO→5000, cam1=AprilTag→5010")
    p.add_argument("--udp-host", default="127.0.0.1", help="UDP target host")
    p.add_argument("--yolo-port", type=int, default=5000, help="UDP port for YOLO (cam0)")
    p.add_argument("--tag-port", type=int, default=5010, help="UDP port for AprilTag (cam1)")
    p.add_argument("--width", type=int, default=640, help="Capture width")
    p.add_argument("--height", type=int, default=480, help="Capture height")
    p.add_argument("--model", default="yolo26s.pt", help="YOLO model")
    p.add_argument("--conf", type=float, default=0.4, help="YOLO confidence threshold")
    p.add_argument("--imgsz", type=int, default=416, help="YOLO inference size")
    p.add_argument("--tag-size", type=float, default=0.12, help="AprilTag size (m)")
    p.add_argument("--tag-id", type=int, default=-1, help="AprilTag ID filter (-1 = any)")
    p.add_argument("--fx", type=float, default=915.0, help="Camera fx for AprilTag (cam1)")
    p.add_argument("--fy", type=float, default=915.0, help="Camera fy")
    p.add_argument("--cx", type=float, default=320.0, help="Camera cx (default half width)")
    p.add_argument("--cy", type=float, default=240.0, help="Camera cy (default half height)")
    p.add_argument("--no-yolo", action="store_true", help="Disable YOLO (do not open cam0)")
    p.add_argument("--no-tag", action="store_true", help="Disable AprilTag (do not open cam1)")
    p.add_argument("--show", action="store_true", help="Show cv2 windows (cam0 YOLO, cam1 AprilTag)")
    p.add_argument("--opencv", action="store_true", help="Force OpenCV backend")
    args = p.parse_args()

    show_display = args.show or bool(os.environ.get("DISPLAY"))

    if args.no_yolo and args.no_tag:
        print("At least one of YOLO or AprilTag must be enabled", file=sys.stderr)
        sys.exit(1)

    use_picam = USE_PICAMERA2 and not args.opencv
    make_cam = _make_picam
    read_cam = (lambda c: _read_picam(c, True))
    backend = "picamera2"

    cam0 = None
    cam1 = None
    if not args.no_yolo:
        cam0, err = make_cam(0, args.width, args.height)
        if cam0 is None:
            print(f"Cannot open camera 0 (YOLO): {err}", file=sys.stderr)
            sys.exit(1)
    if not args.no_tag:
        cam1, err = make_cam(1, args.width, args.height
[truncated — 3459 more characters]
```

### cprobot/src/control/package.xml

```xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>control</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
  <license>TODO: License declaration</license>

  <depend>rclpy</depend>
  <depend>std_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>

```

### cprobot/src/arduino_bridge/package.xml

```xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>arduino_bridge</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="root@todo.todo">root</maintainer>
  <license>TODO: License declaration</license>

  <depend>rclpy</depend>
  <depend>std_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>

```

### cprobot/src/perception/package.xml

```xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>perception</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
  <license>TODO: License declaration</license>

  <depend>rclpy</depend>
  <depend>std_msgs</depend>
  <depend>geometry_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>

```

[19 more indexed source files omitted to keep this export small. The full file list is in the Codebase structure section above.]