ROSonicCam

πŸ“˜ Live Project Website

ROSonicCam

ESP32 + micro-ROS 2 node featuring directional ultrasonic sensing, Kalman filtering, FreeRTOS scheduling, and servo camera control for robotics and drones.

Build ROS2 ESP32 FreeRTOS Ultrasonic Kalman Filter Servo Control Robotics Drones Obstacle Avoidance

Circuit diagram

System Overview

Features

  1. Clone the repo
  2. Open in PlatformIO (VS Code or CLI)
  3. Upload to ESP32 with one click
  4. Connect micro-ROS agent
  5. Done βœ…

πŸ“‹ Table of Contents

  1. Coverage Pattern
  2. System Architecture
  3. Real-Time OS Implementation
  4. Kalman Filter Algorithm
  5. Hardware Setup
  6. PlatformIO Build & Deployment
  7. ROS 2 Subscriber Examples
  8. API Reference
  9. Troubleshooting
  10. License

1. Coverage Pattern

5-directional sensing configuration:

graph LR
    F[Forward ] --> C[Controller]
    L[Left ] --> C
    R[Right ] --> C
    B[Back ] --> C
    D[Downward ] --> C

Key Specifications:

2. System Architecture 

flowchart TB
    S1["Downward Sensor (HC-SR04)"] --> SP
    S2["Forward Sensor (HC-SR04)"] --> SP
    S3["Left Sensor (HC-SR04)"] --> SP
    S4["Right Sensor (HC-SR04)"] --> SP
    S5["Back Sensor (HC-SR04)"] --> SP

    subgraph ESP32["ESP32-WROOM"]
        direction TB
        SP["Sensor Polling\n(20Hz)"] --> KF["Kalman Filter\n(Adaptive)"]
        KF --> RP["ROS 2 Publisher"]
        RP --> M["micro-ROS Client"]
    end



    M["micro-ROS Client\n(Serial/UDP)"] --> R["ROS 2 Agent"]
    R --> T1["Topic: /ultrasonic_sensor/downward/filtered"]
    R --> T2["Topic: /ultrasonic_sensor/forward/filtered"]
    R --> T3["Topic: /ultrasonic_sensor/left/filtered"]
    R --> T4["Topic: /ultrasonic_sensor/right/filtered"]
    R --> T5["Topic: /ultrasonic_sensor/back/filtered"]

3. Real-Time OS Implementation

FreeRTOS Task Scheduling

Task Priority Frequency Execution Time Description
SensorPollTask 3 (High) 20Hz 5-10ms Reads all 5 sensors sequentially
KalmanFilterTask 3 (High) 20Hz 1-2ms/sensor Processes each sensor reading
PublishTask 2 20Hz 3-5ms Publishes to ROS topics
MainLoop 1 (Low) 10Hz Variable Services and diagnostics

Scheduling Algorithm: Preemptive Priority-based Scheduling

FreeRTOS implements a preemptive, priority-based scheduling algorithm with the following characteristics:

flowchart LR
    S[Scheduler] --> P[Priority Queue]
    P --> H[Highest Priority Ready Task]
    H --> R[Running]
    R -->|Preemption| NP[New Higher Priority Task?]
    NP -->|Yes| H
    NP -->|No| C[Continue Execution]
    C -->|Block/Suspend| W[Wait for Event]
    W --> T[Time or Event Trigger]
    T --> P

Key Scheduling Principles:

  1. Priority-Based: Tasks with higher priority (higher number) always preempt lower priority tasks
  2. Preemptive: Running task is immediately interrupted when higher priority task becomes ready
  3. Round-Robin: Tasks of equal priority share CPU time in time slices (configurable)
  4. Deterministic: Worst-case execution times (WCET) guarantee real-time performance

Scheduling Sequence for SkySonar

gantt
    title FreeRTOS Task Execution Timeline (50ms Period)
    dateFormat  ms
    axisFormat %S.%L
    section Tasks
    SensorPollTask   :a1, 0, 10ms
    KalmanFilterTask :a2, after a1, 10ms
    PublishTask      :a3, after a2, 5ms
    MainLoop         :a4, 0, 35ms

Timeline Explanation:

  1. 0-10ms: SensorPollTask (Prio 3) reads all 5 sensors
  2. 10-20ms: KalmanFilterTask (Prio 3) processes sensor data
  3. 20-25ms: PublishTask (Prio 2) sends data to ROS
  4. 0-35ms: MainLoop (Prio 1) runs in background when CPU available

Inter-Task Communication

Mutex Protection for Shared Data

// Acquire mutex before accessing shared data
xSemaphoreTake(data_mutex, portMAX_DELAY);

// Critical section: Update sensor data
raw_readings[0] = sensor_value_0;
// ... update all sensors

// Release mutex when done
xSemaphoreGive(data_mutex);

Task Synchronization

// In SensorPollTask after reading all sensors:
xTaskNotifyGive(KalmanFilterTask);  // Trigger Kalman processing

// In KalmanFilterTask:
ulTaskNotifyTake(pdTRUE, portMAX_DELAY);  // Wait for notification

Synchronization Flow:

  1. Sensor task completes readings and notifies Kalman task
  2. Kalman task wakes immediately (same priority)
  3. Publishing task waits until Kalman completes
  4. Main loop runs only when no other tasks need CPU

Real-Time Performance Analysis

Schedulability Test:

Total CPU Utilization = Ξ£(Task Execution Time / Period)
= (10ms/50ms) + (10ms/50ms) + (5ms/50ms) + (35ms/100ms)
= 0.2 + 0.2 + 0.1 + 0.35 = 0.85 (85% < 100%)

System is schedulable with 15% idle margin

Worst-Case Scenario:

Key Design Considerations

  1. Priority Assignment:
    • Sensor and Kalman tasks at same highest priority
    • Publishing at medium priority
    • System services at lowest priority
  2. Minimizing Latency:
    • Direct task notification for immediate wakeup
    • Mutex protection with minimal critical sections
    • Avoidance of priority inversion
  3. Determinism:
    • Worst-case execution time measurement
    • Fixed-size buffers (no dynamic allocation)
    • Minimal interrupt usage
  4. Robustness:
    • Timeout handling on all blocking calls
    • Stack overflow protection
    • Watchdog monitoring

4. Kalman Filter Algorithm

Core Filter Equations

\begin{align*}
\text{Prediction:} & \\
x_{\text{prior}} &= x_{\text{prev}} \\
P_{\text{prior}} &= P_{\text{prev}} + Q \\
\\
\text{Update:} & \\
K &= \frac{P_{\text{prior}}}{P_{\text{prior}} + R} \\
x &= x_{\text{prior}} + K(z - x_{\text{prior}}) \\
P &= (1 - K)P_{\text{prior}}
\end{align*}

Adaptive Noise Tuning

\begin{align*}
\text{Innovation:} & \quad \epsilon = z - x_{\text{prior}} \\
\text{Measurement Noise:} & \quad R = (1 - \alpha)R_{\text{prev}} + \alpha \epsilon^2 \\
\text{Process Noise:} & \quad Q = \max(0.001, 0.1 \times \sigma^2_{\text{window}})
\end{align*}

Implementation Details:

7. ROS 2 Subscriber Examples

Basic Distance Monitor Node

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range

class SensorMonitor(Node):
    def __init__(self):
        super().__init__('sensor_monitor')
        self.subscriptions = []
        
        # Create subscribers for all directions
        directions = ['downward', 'forward', 'left', 'right', 'back']
        for dir in directions:
            sub = self.create_subscription(
                Range,
                f'/ultrasonic_sensor/{dir}/filtered',
                lambda msg, d=dir: self.sensor_callback(msg, d),
                10
            )
            self.subscriptions.append(sub)
        
        self.get_logger().info("Sensor monitor started")

    def sensor_callback(self, msg, direction):
        if not math.isnan(msg.range):
            self.get_logger().info(
                f"{direction.capitalize()}: {msg.range:.2f}m",
                throttle_duration_sec=1  # Limit to 1Hz output
            )

def main():
    rclpy.init()
    node = SensorMonitor()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

8. API Reference

ROS 2 Topics

| Topic | Type | Description | QoS | |β€”β€”-|β€”β€”|β€”β€”β€”β€”-|—–| | /ultrasonic_sensor/downward/filtered | sensor_msgs/Range | Downward distance (m) | Best effort | | /ultrasonic_sensor/forward/filtered | sensor_msgs/Range | Forward distance (m) | Best effort | | /ultrasonic_sensor/left/filtered | sensor_msgs/Range | Left distance (m) | Best effort | | /ultrasonic_sensor/right/filtered | sensor_msgs/Range | Right distance (m) | Best effort | | /ultrasonic_sensor/back/filtered | sensor_msgs/Range | Back distance (m) | Best effort | | /diagnostics | diagnostic_msgs/DiagnosticStatus | System health | Reliable |

Service

| Service | Type | Description | |β€”β€”β€”|β€”β€”|β€”β€”β€”β€”-| | /servo_cam_service | servocam_interfaces/srv/Servocam | Pan/tilt control |

9. Building, Flashing, and Operating SkySonar

PlatformIO Setup

platformio.ini Configuration

[env:upesy_wroom]
platform = espressif32
board = upesy_wroom
framework = arduino
monitor_speed = 115200
lib_deps = 
    teckel12/NewPing@^1.9.7
    micro-ROS/micro_ros_platformio@^0.4.0
build_flags = 
    -I include
    -Wno-unused-variable
    -Wno-unused-parameter

Building and Flashing

  1. Install dependencies: 
    pio lib install
  2. Build the firmware: 
    pio run
  3. Flash to ESP32: 
    pio run -t upload
  4. Monitor serial output: 
    pio device monitor

Micro-ROS Agent Setup

Install micro-ROS Agent

# For ROS 2 Humble
sudo apt install ros-humble-micro-ros-agent

# For ROS 2 Foxy
sudo apt install ros-foxy-micro-ros-agent

Start Agent

ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 -b 115200

System Operation

Startup Sequence

  1. Hardware initialization
  2. Sensor diagnostic check
  3. FreeRTOS task creation
  4. micro-ROS node initialization
  5. Continuous sensor reading and publishing

Expected Serial Output

Starting Ultrasonic Sensor System...

--- Sensor Status Report ---
Sensor downward (TRIG:4, ECHO:16): OK (1.23m)
Sensor forward (TRIG:17, ECHO:14): OK (0.87m)
...
----------------------------

System initialization complete!
micro-ROS connection active
Hardware status: ALL SENSORS OK
Servo status: CONFIGURED

Topics:
  /ultrasonic_sensor/downward/raw
  /ultrasonic_sensor/downward/filtered
  ...
Service: /servo_cam_service

[downward] Raw: 1.23m | Filtered: 1.22m
[forward] Raw: 0.87m | Filtered: 0.86m
...

10. Troubleshooting

Troubleshooting No ROSΒ 2 topicsCause: micro-ROS agent not running or wrong portSolution: Start agent with:


ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyUSB0 -b115200

Intermittent readingsCause: Voltage divider mismatch or loose wiringSolution: Verify 1β€―kΞ©/2β€―kΞ© resistor divider and secure all sensor connections.

Slow Kalman convergenceCause: Low adaptation rate (Ξ± too small)Solution: Increase Ξ± in code (e.g., from 0.01 to 0.02).

Over-filteringCause: Minimum process noise (Q_min) too lowSolution: Raise Q_min (e.g., from 0.001 to 0.01).

Noise spikesCause: Sliding window size too smallSolution: Increase WINDOW_SIZE (e.g., from 10 to 20 samples).

RTOS preemption lagCause: Priority inversion or misconfigured task prioritiesSolution: Ensure FreeRTOS priorities: SensorTask (highest) > PublishTask > main loop.

Servo no responseCause: PWM duty change below detection thresholdSolution: Guarantee angle commands result in β‰₯10 duty unit change