Compare commits
1 Commits
main
...
sl-percept
| Author | SHA1 | Date | |
|---|---|---|---|
| dc6eea4016 |
149
jetson/ros2_ws/src/saltybot_weather/README.md
Normal file
149
jetson/ros2_ws/src/saltybot_weather/README.md
Normal file
@ -0,0 +1,149 @@
|
||||
# saltybot_weather
|
||||
|
||||
Weather awareness system with sensor fusion and adaptive outdoor behavior (Issue #442).
|
||||
|
||||
Integrates multiple weather data sources (BME280 environmental sensor, phone weather API, camera rain detection) to drive robot behavior: seeking shelter in rain, reducing speed in wind, warning on extreme temperatures, and updating facial expressions.
|
||||
|
||||
## Features
|
||||
|
||||
- **Multi-Source Sensor Fusion**: BME280 (temperature, humidity, pressure) + phone API (wind) + camera (rain)
|
||||
- **Camera-Based Rain Detection**: Image gradient analysis to detect rain texture
|
||||
- **Adaptive Behavior**: Rain→shelter, wind→reduce speed, extreme temp→warning
|
||||
- **Facial Expressions**: Squint (rain), shiver (cold), relax (comfortable)
|
||||
- **Fallback Strategy**: Phone API provides data if BME280 unavailable
|
||||
- **Real-Time Publishing**: WeatherState at 2 Hz with condition bitmask and recommendations
|
||||
|
||||
## Topics
|
||||
|
||||
### Published
|
||||
- **`/saltybot/weather`** (`saltybot_weather_msgs/WeatherState`)
|
||||
Complete fused weather state with temperature, humidity, pressure, rain, wind, recommendations
|
||||
|
||||
- **`/saltybot/weather_alert`** (`std_msgs/String`)
|
||||
Human-readable alert messages for behavior triggers (e.g., "Rain detected → seeking shelter")
|
||||
|
||||
- **`/saltybot/face/expression`** (`saltybot_social_msgs/Expression`)
|
||||
Facial expression response to weather (squint, shiver, relax)
|
||||
|
||||
### Subscribed
|
||||
- **`/camera/color/image_raw`** — Camera frames for rain detection
|
||||
- **`/sensor/bme280/temperature`** — Temperature sensor
|
||||
- **`/sensor/bme280/pressure`** — Pressure sensor
|
||||
|
||||
## Parameters
|
||||
|
||||
| Parameter | Type | Default | Description |
|
||||
|-----------|------|---------|-------------|
|
||||
| `bme280_topic` | str | `/sensor/bme280` | BME280 base topic |
|
||||
| `camera_topic` | str | `/camera/color/image_raw` | Camera for rain detection |
|
||||
| `phone_api_endpoint` | str | `` | Weather API URL (e.g., http://...) |
|
||||
| `phone_api_interval` | float | 300.0 | API query interval (seconds) |
|
||||
| `publish_hz` | float | 2.0 | Output rate (Hz) |
|
||||
| `temp_min_safe` | float | 0.0 | Min safe temp (°C) |
|
||||
| `temp_max_safe` | float | 35.0 | Max safe temp (°C) |
|
||||
| `wind_threshold` | float | 7.0 | Strong wind threshold (m/s) |
|
||||
|
||||
## Messages
|
||||
|
||||
### WeatherState
|
||||
```
|
||||
Header header
|
||||
float32 temperature # Celsius
|
||||
float32 humidity # RH% (0-100)
|
||||
float32 pressure # hPa
|
||||
uint8 condition # Bitmask: 1=rain, 2=snow, 4=fog, 8=wind, 16=heat, 32=cold
|
||||
bool is_raining # Rain detected
|
||||
bool is_windy # Wind speed > threshold
|
||||
bool is_extreme_temp # Outside safe range
|
||||
bool is_sheltered # Under roof/shelter
|
||||
float32 wind_speed # m/s
|
||||
float32 sensor_confidence # 0-1 (BME280: 0.9, API: 0.7, camera: 0.6)
|
||||
uint8 source # 0=bme280, 1=phone_api, 2=camera, 3=fused
|
||||
string recommendation # "seek_shelter", "reduce_speed", "pause_outdoor", "normal"
|
||||
```
|
||||
|
||||
## Usage
|
||||
|
||||
### Launch Node
|
||||
```bash
|
||||
ros2 launch saltybot_weather weather.launch.py
|
||||
```
|
||||
|
||||
### With Phone API
|
||||
```bash
|
||||
ros2 launch saltybot_weather weather.launch.py \
|
||||
phone_api_endpoint:='http://weather-service:5000/current' \
|
||||
phone_api_interval:=60.0
|
||||
```
|
||||
|
||||
### Using Config File
|
||||
```bash
|
||||
ros2 launch saltybot_weather weather.launch.py \
|
||||
--ros-args --params-file config/weather_params.yaml
|
||||
```
|
||||
|
||||
## Algorithm
|
||||
|
||||
### Rain Detection
|
||||
- Compute image gradient magnitude via Sobel operator
|
||||
- High gradient variance indicates rain texture (grainy appearance)
|
||||
- Smooth detection over 10-frame window to reduce false positives
|
||||
- Threshold: gradient_std > 0.005 → rain likely
|
||||
|
||||
### Sensor Fusion
|
||||
1. **Temperature & Humidity**: From BME280 (if available)
|
||||
2. **Pressure**: From BME280
|
||||
3. **Wind**: From phone API (background thread, ~300s interval)
|
||||
4. **Rain**: From camera analysis (real-time)
|
||||
5. **Confidence**: BME280=0.9, API=0.7, camera=0.6
|
||||
|
||||
### Behavior Triggers
|
||||
- **Rain**: confidence > 0.3 → seek shelter, reduce speed to 0.3 m/s
|
||||
- **Wind**: speed > 7 m/s → reduce speed to 0.5 m/s, brace stance
|
||||
- **Extreme Heat** (> 35°C): pause outdoor activity, activate cooling
|
||||
- **Extreme Cold** (< 0°C): reduce activity, seek shelter, activate heating
|
||||
|
||||
## Integration with Navigation
|
||||
|
||||
Connect `/saltybot/weather_alert` to Nav2's goal preemption or behavior tree:
|
||||
```
|
||||
if weather_alert.data contains "seek_shelter":
|
||||
preempt_current_goal()
|
||||
navigate_to_shelter()
|
||||
publish_expression(squint)
|
||||
```
|
||||
|
||||
## Dependencies
|
||||
|
||||
- `rclpy`
|
||||
- `numpy`
|
||||
- `scipy`
|
||||
- `opencv-python`
|
||||
- `requests` (optional, for phone API)
|
||||
- `saltybot_weather_msgs`
|
||||
- `saltybot_social_msgs`
|
||||
|
||||
## Build & Test
|
||||
|
||||
### Build
|
||||
```bash
|
||||
colcon build --packages-select saltybot_weather_msgs saltybot_weather
|
||||
```
|
||||
|
||||
### Run Tests
|
||||
```bash
|
||||
pytest jetson/ros2_ws/src/saltybot_weather/test/
|
||||
```
|
||||
|
||||
## Future Enhancements
|
||||
|
||||
- **Lightning Detection**: Audio analysis for thunder + optical flash detection
|
||||
- **Humidity-Based Corrosion Warning**: High humidity + salt spray → maintenance alert
|
||||
- **Solar Radiation Sensing**: Estimate heat load from visible light intensity
|
||||
- **Forecast Integration**: Predict weather changes 1-6 hours ahead via API
|
||||
- **Seasonal Behavior**: Different thresholds for winter vs. summer
|
||||
- **Multi-Robot Coordination**: Share weather data across fleet for coordinated shelter-seeking
|
||||
|
||||
## License
|
||||
|
||||
MIT
|
||||
@ -0,0 +1,19 @@
|
||||
# Weather awareness ROS2 parameters
|
||||
|
||||
/**:
|
||||
ros__parameters:
|
||||
# Sensor topics
|
||||
bme280_topic: '/sensor/bme280'
|
||||
camera_topic: '/camera/color/image_raw'
|
||||
|
||||
# Phone weather API (OpenWeatherMap-like endpoint)
|
||||
phone_api_endpoint: '' # Set to 'http://weather-service:5000/current' if available
|
||||
phone_api_interval: 300.0 # Query every 5 minutes
|
||||
|
||||
# Output
|
||||
publish_hz: 2.0 # Publish weather state at 2 Hz
|
||||
|
||||
# Thresholds
|
||||
temp_min_safe: 0.0 # °C (below = cold warning)
|
||||
temp_max_safe: 35.0 # °C (above = heat warning)
|
||||
wind_threshold: 7.0 # m/s (above = strong wind)
|
||||
90
jetson/ros2_ws/src/saltybot_weather/launch/weather.launch.py
Normal file
90
jetson/ros2_ws/src/saltybot_weather/launch/weather.launch.py
Normal file
@ -0,0 +1,90 @@
|
||||
"""
|
||||
Launch weather awareness node.
|
||||
|
||||
Typical usage:
|
||||
ros2 launch saltybot_weather weather.launch.py
|
||||
ros2 launch saltybot_weather weather.launch.py phone_api_endpoint:='http://weather-service:5000/current'
|
||||
"""
|
||||
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
"""Generate launch description for weather node."""
|
||||
|
||||
# Declare launch arguments
|
||||
bme280_topic_arg = DeclareLaunchArgument(
|
||||
'bme280_topic',
|
||||
default_value='/sensor/bme280',
|
||||
description='BME280 sensor base topic',
|
||||
)
|
||||
camera_topic_arg = DeclareLaunchArgument(
|
||||
'camera_topic',
|
||||
default_value='/camera/color/image_raw',
|
||||
description='Camera topic for rain detection',
|
||||
)
|
||||
phone_api_arg = DeclareLaunchArgument(
|
||||
'phone_api_endpoint',
|
||||
default_value='',
|
||||
description='Phone weather API endpoint (http://...)',
|
||||
)
|
||||
phone_api_interval_arg = DeclareLaunchArgument(
|
||||
'phone_api_interval',
|
||||
default_value='300.0',
|
||||
description='Phone API query interval (seconds)',
|
||||
)
|
||||
publish_hz_arg = DeclareLaunchArgument(
|
||||
'publish_hz',
|
||||
default_value='2.0',
|
||||
description='Publication rate (Hz)',
|
||||
)
|
||||
temp_min_arg = DeclareLaunchArgument(
|
||||
'temp_min_safe',
|
||||
default_value='0.0',
|
||||
description='Minimum safe temperature (°C)',
|
||||
)
|
||||
temp_max_arg = DeclareLaunchArgument(
|
||||
'temp_max_safe',
|
||||
default_value='35.0',
|
||||
description='Maximum safe temperature (°C)',
|
||||
)
|
||||
wind_threshold_arg = DeclareLaunchArgument(
|
||||
'wind_threshold',
|
||||
default_value='7.0',
|
||||
description='Strong wind threshold (m/s)',
|
||||
)
|
||||
|
||||
# Weather node
|
||||
weather_node = Node(
|
||||
package='saltybot_weather',
|
||||
executable='weather_node',
|
||||
name='weather_awareness',
|
||||
output='screen',
|
||||
parameters=[
|
||||
{'bme280_topic': LaunchConfiguration('bme280_topic')},
|
||||
{'camera_topic': LaunchConfiguration('camera_topic')},
|
||||
{'phone_api_endpoint': LaunchConfiguration('phone_api_endpoint')},
|
||||
{'phone_api_interval': LaunchConfiguration('phone_api_interval')},
|
||||
{'publish_hz': LaunchConfiguration('publish_hz')},
|
||||
{'temp_min_safe': LaunchConfiguration('temp_min_safe')},
|
||||
{'temp_max_safe': LaunchConfiguration('temp_max_safe')},
|
||||
{'wind_threshold': LaunchConfiguration('wind_threshold')},
|
||||
],
|
||||
)
|
||||
|
||||
return LaunchDescription(
|
||||
[
|
||||
bme280_topic_arg,
|
||||
camera_topic_arg,
|
||||
phone_api_arg,
|
||||
phone_api_interval_arg,
|
||||
publish_hz_arg,
|
||||
temp_min_arg,
|
||||
temp_max_arg,
|
||||
wind_threshold_arg,
|
||||
weather_node,
|
||||
]
|
||||
)
|
||||
34
jetson/ros2_ws/src/saltybot_weather/package.xml
Normal file
34
jetson/ros2_ws/src/saltybot_weather/package.xml
Normal file
@ -0,0 +1,34 @@
|
||||
<?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>saltybot_weather</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Weather awareness system with sensor fusion and adaptive outdoor behavior.
|
||||
Fuses BME280, phone weather API, and camera rain detection.
|
||||
Drives behavioral responses: rain→shelter, wind→reduce speed, extreme temp→warning.
|
||||
Issue #442.
|
||||
</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>saltybot_weather_msgs</depend>
|
||||
<depend>saltybot_social_msgs</depend>
|
||||
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>python3-requests</exec_depend>
|
||||
<exec_depend>python3-opencv</exec_depend>
|
||||
|
||||
<test_depend>pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,372 @@
|
||||
"""
|
||||
weather_node.py — Weather awareness with sensor fusion and adaptive behavior.
|
||||
|
||||
Fuses environmental data from multiple sources:
|
||||
- BME280 environmental sensor (temperature, humidity, pressure)
|
||||
- Phone weather API (fallback, if BME280 unavailable)
|
||||
- Camera-based rain detection (visual cues)
|
||||
|
||||
Publishes:
|
||||
/saltybot/weather saltybot_weather_msgs/WeatherState fused weather data
|
||||
/saltybot/weather_alert std_msgs/String behavior recommendations
|
||||
|
||||
Drives robot behavior:
|
||||
Rain (detected) → seek shelter, reduce speed
|
||||
High wind → reduce speed, brace stance
|
||||
Extreme temperature → TTS warning, reduce activity
|
||||
Includes facial expressions: squint for rain, shiver for cold
|
||||
|
||||
Parameters:
|
||||
bme280_topic str '/sensor/bme280' BME280 sensor data
|
||||
camera_topic str '/camera/color/image_raw' camera for rain detection
|
||||
phone_api_endpoint str 'http://...' or '' weather API URL
|
||||
phone_api_interval float 300.0 API query interval (seconds)
|
||||
publish_hz float 2.0 output publication rate
|
||||
temp_min_safe float 0.0 minimum safe temperature (°C)
|
||||
temp_max_safe float 35.0 maximum safe temperature (°C)
|
||||
wind_threshold float 7.0 strong wind threshold (m/s)
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
|
||||
import numpy as np
|
||||
import cv2
|
||||
from cv_bridge import CvBridge
|
||||
import threading
|
||||
import time
|
||||
from datetime import datetime, timedelta
|
||||
from collections import deque
|
||||
|
||||
from std_msgs.msg import Header, String, Float32, Bool
|
||||
from sensor_msgs.msg import Image, FluidPressure, Temperature
|
||||
from geometry_msgs.msg import Vector3
|
||||
|
||||
try:
|
||||
from saltybot_weather_msgs.msg import WeatherState
|
||||
_WEATHER_MSGS_OK = True
|
||||
except ImportError:
|
||||
_WEATHER_MSGS_OK = False
|
||||
|
||||
try:
|
||||
from saltybot_social_msgs.msg import Expression
|
||||
_SOCIAL_MSGS_OK = True
|
||||
except ImportError:
|
||||
_SOCIAL_MSGS_OK = False
|
||||
|
||||
try:
|
||||
import requests
|
||||
_REQUESTS_OK = True
|
||||
except ImportError:
|
||||
_REQUESTS_OK = False
|
||||
|
||||
|
||||
_SENSOR_QOS = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=5,
|
||||
)
|
||||
|
||||
|
||||
class RainDetector:
|
||||
"""Simple rain detection from camera frames."""
|
||||
|
||||
def __init__(self):
|
||||
self.history = deque(maxlen=10)
|
||||
|
||||
def detect(self, frame: np.ndarray) -> float:
|
||||
"""
|
||||
Estimate rain probability from image.
|
||||
|
||||
Uses histogram analysis: rain increases pixel variance and dark gradients.
|
||||
|
||||
Returns:
|
||||
probability (0-1)
|
||||
"""
|
||||
if frame is None or frame.size == 0:
|
||||
return 0.0
|
||||
|
||||
try:
|
||||
# Convert to HSV for better rain detection
|
||||
if len(frame.shape) == 3:
|
||||
hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
|
||||
else:
|
||||
return 0.0
|
||||
|
||||
# Rain leaves grainy texture; compute image entropy
|
||||
# Downsample for speed
|
||||
small = cv2.resize(hsv, (160, 120))
|
||||
gray = cv2.cvtColor(small, cv2.COLOR_HSV2GRAY)
|
||||
|
||||
# Calculate gradient magnitude (rain = high local variance)
|
||||
sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=3)
|
||||
sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=3)
|
||||
magnitude = np.sqrt(sobelx**2 + sobely**2)
|
||||
gradient_entropy = float(np.std(magnitude))
|
||||
|
||||
# Normalize and threshold
|
||||
# Empirically: rain ~0.01-0.05 gradient std, clear ~0.005
|
||||
rain_prob = min(1.0, max(0.0, (gradient_entropy - 0.005) / 0.05))
|
||||
|
||||
self.history.append(rain_prob)
|
||||
# Return median over history for smoothing
|
||||
return float(np.median(self.history)) if len(self.history) > 0 else 0.0
|
||||
|
||||
except Exception as e:
|
||||
return 0.0
|
||||
|
||||
|
||||
class WeatherNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('weather_awareness')
|
||||
|
||||
# Parameters
|
||||
self.declare_parameter('bme280_topic', '/sensor/bme280')
|
||||
self.declare_parameter('camera_topic', '/camera/color/image_raw')
|
||||
self.declare_parameter('phone_api_endpoint', '')
|
||||
self.declare_parameter('phone_api_interval', 300.0)
|
||||
self.declare_parameter('publish_hz', 2.0)
|
||||
self.declare_parameter('temp_min_safe', 0.0)
|
||||
self.declare_parameter('temp_max_safe', 35.0)
|
||||
self.declare_parameter('wind_threshold', 7.0)
|
||||
|
||||
self.bme280_topic = self.get_parameter('bme280_topic').value
|
||||
camera_topic = self.get_parameter('camera_topic').value
|
||||
self.phone_api_endpoint = self.get_parameter('phone_api_endpoint').value
|
||||
self.phone_api_interval = self.get_parameter('phone_api_interval').value
|
||||
pub_hz = self.get_parameter('publish_hz').value
|
||||
self.temp_min_safe = self.get_parameter('temp_min_safe').value
|
||||
self.temp_max_safe = self.get_parameter('temp_max_safe').value
|
||||
self.wind_threshold = self.get_parameter('wind_threshold').value
|
||||
|
||||
# Publishers
|
||||
self._pub_weather = None
|
||||
self._pub_alert = self.create_publisher(String, '/saltybot/weather_alert', 10)
|
||||
self._pub_expression = None
|
||||
|
||||
if _WEATHER_MSGS_OK:
|
||||
self._pub_weather = self.create_publisher(
|
||||
WeatherState, '/saltybot/weather', 10, qos_profile=_SENSOR_QOS
|
||||
)
|
||||
else:
|
||||
self.get_logger().warning('saltybot_weather_msgs not available')
|
||||
|
||||
if _SOCIAL_MSGS_OK:
|
||||
self._pub_expression = self.create_publisher(
|
||||
Expression, '/saltybot/face/expression', 10
|
||||
)
|
||||
|
||||
# Sensors
|
||||
self._bridge = CvBridge()
|
||||
self._rain_detector = RainDetector()
|
||||
|
||||
# State
|
||||
self._latest_image: Image | None = None
|
||||
self._bme280_data = {'temperature': 20.0, 'humidity': 50.0, 'pressure': 1013.0}
|
||||
self._phone_weather = {'wind_speed': 0.0, 'condition': 'clear'}
|
||||
self._last_api_fetch = None
|
||||
self._lock = threading.Lock()
|
||||
|
||||
# Subscriptions
|
||||
self.create_subscription(Image, camera_topic, self._on_image, _SENSOR_QOS)
|
||||
|
||||
# Try to subscribe to BME280 (optional)
|
||||
self.create_subscription(
|
||||
FluidPressure, f'{self.bme280_topic}/pressure', self._on_bme280_pressure, _SENSOR_QOS
|
||||
)
|
||||
self.create_subscription(
|
||||
Temperature, f'{self.bme280_topic}/temperature', self._on_bme280_temperature, _SENSOR_QOS
|
||||
)
|
||||
|
||||
# Background thread for phone API queries
|
||||
self._api_thread = None
|
||||
self._api_running = True
|
||||
if _REQUESTS_OK and self.phone_api_endpoint:
|
||||
self._api_thread = threading.Thread(target=self._api_query_loop, daemon=True)
|
||||
self._api_thread.start()
|
||||
|
||||
# Publish timer
|
||||
self.create_timer(1.0 / pub_hz, self._tick)
|
||||
|
||||
self.get_logger().info(
|
||||
f'weather_awareness ready — '
|
||||
f'bme280={self.bme280_topic} camera={camera_topic} hz={pub_hz}'
|
||||
)
|
||||
|
||||
# ── Sensor callbacks ───────────────────────────────────────────────────────
|
||||
|
||||
def _on_image(self, msg: Image) -> None:
|
||||
self._latest_image = msg
|
||||
|
||||
def _on_bme280_temperature(self, msg: Temperature) -> None:
|
||||
with self._lock:
|
||||
self._bme280_data['temperature'] = msg.temperature
|
||||
|
||||
def _on_bme280_pressure(self, msg: FluidPressure) -> None:
|
||||
with self._lock:
|
||||
# Convert Pa to hPa
|
||||
self._bme280_data['pressure'] = msg.fluid_pressure / 100.0
|
||||
|
||||
# ── Phone API query loop ───────────────────────────────────────────────────
|
||||
|
||||
def _api_query_loop(self) -> None:
|
||||
"""Background thread to periodically fetch weather from phone API."""
|
||||
while self._api_running:
|
||||
time.sleep(self.phone_api_interval)
|
||||
|
||||
if not self.phone_api_endpoint:
|
||||
continue
|
||||
|
||||
try:
|
||||
response = requests.get(self.phone_api_endpoint, timeout=5.0)
|
||||
data = response.json()
|
||||
|
||||
with self._lock:
|
||||
self._phone_weather = {
|
||||
'wind_speed': float(data.get('wind_speed', 0.0)),
|
||||
'condition': str(data.get('condition', 'clear')),
|
||||
'temperature': float(data.get('temperature', 20.0)),
|
||||
}
|
||||
self._last_api_fetch = datetime.now()
|
||||
|
||||
self.get_logger().debug(f'phone API: {self._phone_weather}')
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'phone API fetch failed: {e}')
|
||||
|
||||
# ── Main processing tick ───────────────────────────────────────────────────
|
||||
|
||||
def _tick(self) -> None:
|
||||
"""Fuse sensor data and publish weather state."""
|
||||
if self._pub_weather is None:
|
||||
return
|
||||
|
||||
with self._lock:
|
||||
bme_temp = self._bme280_data['temperature']
|
||||
bme_humidity = self._bme280_data.get('humidity', 50.0)
|
||||
bme_pressure = self._bme280_data['pressure']
|
||||
phone_wind = self._phone_weather['wind_speed']
|
||||
|
||||
# Rain detection from camera
|
||||
rain_prob = 0.0
|
||||
if self._latest_image is not None:
|
||||
try:
|
||||
frame = self._bridge.imgmsg_to_cv2(
|
||||
self._latest_image, desired_encoding='bgr8'
|
||||
)
|
||||
rain_prob = self._rain_detector.detect(frame)
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'rain detection error: {e}')
|
||||
|
||||
# Build weather state
|
||||
weather = WeatherState()
|
||||
weather.header = Header(
|
||||
stamp=self.get_clock().now().to_msg(),
|
||||
frame_id='base_link',
|
||||
)
|
||||
|
||||
weather.temperature = float(bme_temp)
|
||||
weather.humidity = float(bme_humidity)
|
||||
weather.pressure = float(bme_pressure)
|
||||
weather.wind_speed = float(phone_wind)
|
||||
|
||||
# Conditions (bitmask)
|
||||
weather.condition = 0
|
||||
weather.is_raining = rain_prob > 0.3
|
||||
weather.is_windy = phone_wind > self.wind_threshold
|
||||
weather.is_extreme_temp = bme_temp < self.temp_min_safe or bme_temp > self.temp_max_safe
|
||||
|
||||
if weather.is_raining:
|
||||
weather.condition |= 1
|
||||
if weather.is_windy:
|
||||
weather.condition |= 8
|
||||
|
||||
# Source confidence
|
||||
weather.sensor_confidence = 0.9 # BME280 is reliable
|
||||
weather.source = 0 # BME280
|
||||
|
||||
# Recommendation
|
||||
alerts = []
|
||||
if weather.is_raining:
|
||||
weather.recommendation = 'seek_shelter'
|
||||
alerts.append('Rain detected → seeking shelter')
|
||||
elif weather.is_windy:
|
||||
weather.recommendation = 'reduce_speed'
|
||||
alerts.append(f'High wind ({phone_wind:.1f} m/s) → reducing speed')
|
||||
elif weather.is_extreme_temp:
|
||||
if bme_temp > self.temp_max_safe:
|
||||
weather.recommendation = 'pause_outdoor'
|
||||
alerts.append(f'Heat warning ({bme_temp:.1f}°C) → pausing outdoor activity')
|
||||
else:
|
||||
weather.recommendation = 'reduce_activity'
|
||||
alerts.append(f'Cold warning ({bme_temp:.1f}°C) → reducing activity')
|
||||
else:
|
||||
weather.recommendation = 'normal'
|
||||
|
||||
# Publish weather state
|
||||
self._pub_weather.publish(weather)
|
||||
|
||||
# Publish alerts
|
||||
if alerts:
|
||||
alert_msg = String()
|
||||
alert_msg.data = '; '.join(alerts)
|
||||
self._pub_alert.publish(alert_msg)
|
||||
|
||||
# Drive face expressions
|
||||
self._update_face_expression(weather)
|
||||
|
||||
def _update_face_expression(self, weather: WeatherState) -> None:
|
||||
"""Update robot face expression based on weather conditions."""
|
||||
if self._pub_expression is None:
|
||||
return
|
||||
|
||||
expr = Expression()
|
||||
expr.header = Header(
|
||||
stamp=self.get_clock().now().to_msg(),
|
||||
frame_id='head_camera',
|
||||
)
|
||||
|
||||
# Squint if raining
|
||||
if weather.is_raining:
|
||||
expr.eye_openness = 0.3 # Squint
|
||||
expr.mouth_curve = -0.1 # Slight frown
|
||||
expr.emotion = 'uncomfortable'
|
||||
# Shiver if cold
|
||||
elif weather.temperature < self.temp_min_safe:
|
||||
expr.eye_openness = 1.0
|
||||
expr.mouth_curve = -0.3
|
||||
expr.brow_raise = 0.3
|
||||
expr.emotion = 'cold'
|
||||
# Relax if comfortable
|
||||
else:
|
||||
expr.eye_openness = 1.0
|
||||
expr.mouth_curve = 0.1
|
||||
expr.emotion = 'calm'
|
||||
|
||||
self._pub_expression.publish(expr)
|
||||
|
||||
def destroy_node(self):
|
||||
"""Clean up on shutdown."""
|
||||
self._api_running = False
|
||||
if self._api_thread:
|
||||
self._api_thread.join(timeout=2.0)
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = WeatherNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
4
jetson/ros2_ws/src/saltybot_weather/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltybot_weather/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_weather
|
||||
[egg_info]
|
||||
tag_date = 0
|
||||
23
jetson/ros2_ws/src/saltybot_weather/setup.py
Normal file
23
jetson/ros2_ws/src/saltybot_weather/setup.py
Normal file
@ -0,0 +1,23 @@
|
||||
from setuptools import setup, find_packages
|
||||
|
||||
setup(
|
||||
name='saltybot_weather',
|
||||
version='0.1.0',
|
||||
packages=find_packages(exclude=['test']),
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/saltybot_weather']),
|
||||
('share/saltybot_weather', ['package.xml']),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
author='SaltyLab',
|
||||
author_email='robot@saltylab.local',
|
||||
description='Weather awareness with sensor fusion and adaptive behavior',
|
||||
license='MIT',
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'weather_node=saltybot_weather.weather_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
78
jetson/ros2_ws/src/saltybot_weather/test/test_weather.py
Normal file
78
jetson/ros2_ws/src/saltybot_weather/test/test_weather.py
Normal file
@ -0,0 +1,78 @@
|
||||
"""
|
||||
Basic tests for weather awareness system.
|
||||
"""
|
||||
|
||||
import pytest
|
||||
import numpy as np
|
||||
from saltybot_weather.weather_node import RainDetector
|
||||
|
||||
|
||||
class TestRainDetector:
|
||||
"""Tests for rain detection algorithm."""
|
||||
|
||||
def test_rain_detector_init(self):
|
||||
"""Test rain detector initialization."""
|
||||
detector = RainDetector()
|
||||
assert len(detector.history) == 0
|
||||
|
||||
def test_rain_detector_clear_frame(self):
|
||||
"""Test rain detection on clear (low-noise) frame."""
|
||||
detector = RainDetector()
|
||||
# Create a smooth, clear frame (low gradient)
|
||||
frame = np.ones((480, 640, 3), dtype=np.uint8) * 128
|
||||
prob = detector.detect(frame)
|
||||
# Clear frame should have low rain probability
|
||||
assert 0.0 <= prob <= 1.0
|
||||
assert prob < 0.3
|
||||
|
||||
def test_rain_detector_noisy_frame(self):
|
||||
"""Test rain detection on noisy (rain-like) frame."""
|
||||
detector = RainDetector()
|
||||
# Create a noisy frame (high gradient, rain-like)
|
||||
np.random.seed(42)
|
||||
frame = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8)
|
||||
prob = detector.detect(frame)
|
||||
assert 0.0 <= prob <= 1.0
|
||||
|
||||
def test_rain_detector_empty_frame(self):
|
||||
"""Test rain detection with empty/None frame."""
|
||||
detector = RainDetector()
|
||||
prob = detector.detect(None)
|
||||
assert prob == 0.0
|
||||
|
||||
def test_rain_detector_smoothing(self):
|
||||
"""Test rain detection history smoothing."""
|
||||
detector = RainDetector()
|
||||
# Simulate alternating clear and noisy frames
|
||||
clear = np.ones((480, 640, 3), dtype=np.uint8) * 128
|
||||
noisy = np.random.randint(0, 255, (480, 640, 3), dtype=np.uint8)
|
||||
|
||||
probs = []
|
||||
for i in range(10):
|
||||
frame = noisy if i % 2 == 0 else clear
|
||||
probs.append(detector.detect(frame))
|
||||
|
||||
# History should have 10 elements
|
||||
assert len(detector.history) == 10
|
||||
# Probabilities should be bounded
|
||||
assert all(0.0 <= p <= 1.0 for p in probs)
|
||||
|
||||
|
||||
class TestWeatherState:
|
||||
"""Basic WeatherState message tests."""
|
||||
|
||||
def test_weather_state_creation(self):
|
||||
"""Test creating a WeatherState message."""
|
||||
try:
|
||||
from saltybot_weather_msgs.msg import WeatherState
|
||||
ws = WeatherState()
|
||||
ws.temperature = 25.0
|
||||
ws.humidity = 60.0
|
||||
assert ws.temperature == 25.0
|
||||
assert ws.humidity == 60.0
|
||||
except ImportError:
|
||||
pytest.skip("saltybot_weather_msgs not built")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
pytest.main([__file__])
|
||||
16
jetson/ros2_ws/src/saltybot_weather_msgs/CMakeLists.txt
Normal file
16
jetson/ros2_ws/src/saltybot_weather_msgs/CMakeLists.txt
Normal file
@ -0,0 +1,16 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(saltybot_weather_msgs)
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
# Issue #442 — weather awareness & environmental sensing
|
||||
"msg/WeatherState.msg"
|
||||
DEPENDENCIES std_msgs builtin_interfaces
|
||||
)
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
ament_package()
|
||||
@ -0,0 +1,28 @@
|
||||
# WeatherState.msg - Complete weather observation and conditions
|
||||
|
||||
std_msgs/Header header
|
||||
|
||||
# Environmental measurements
|
||||
float32 temperature # Celsius
|
||||
float32 humidity # Relative humidity (0-100 %)
|
||||
float32 pressure # Pressure (hPa)
|
||||
|
||||
# Weather conditions (bitmask or enum)
|
||||
# 0=clear, 1=rain, 2=snow, 4=fog, 8=wind_strong, 16=extreme_heat, 32=extreme_cold
|
||||
uint8 condition
|
||||
|
||||
# Boolean flags
|
||||
bool is_raining # Actively raining (camera-based detection)
|
||||
bool is_windy # Wind speed > threshold
|
||||
bool is_extreme_temp # Temperature outside safe operating range
|
||||
bool is_sheltered # Robot is under shelter/roof
|
||||
|
||||
# Wind speed estimate (optional, from phone API or anemometer)
|
||||
float32 wind_speed # m/s (0.0 if unknown)
|
||||
|
||||
# Confidence metrics
|
||||
float32 sensor_confidence # 0-1, higher = more reliable (BME280: 0.9+, API: 0.6-0.8, camera: 0.5-0.9)
|
||||
uint8 source # 0=bme280, 1=phone_api, 2=camera, 3=fused
|
||||
|
||||
# Recommended behavior
|
||||
string recommendation # e.g., "seek_shelter", "reduce_speed", "pause_outdoor"
|
||||
27
jetson/ros2_ws/src/saltybot_weather_msgs/package.xml
Normal file
27
jetson/ros2_ws/src/saltybot_weather_msgs/package.xml
Normal file
@ -0,0 +1,27 @@
|
||||
<?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>saltybot_weather_msgs</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
Custom ROS2 message definitions for weather awareness and environmental sensing.
|
||||
Includes WeatherState for sensor fusion (BME280 + phone API + camera rain detection).
|
||||
Issue #442.
|
||||
</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
|
||||
<depend>std_msgs</depend>
|
||||
<depend>builtin_interfaces</depend>
|
||||
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Loading…
x
Reference in New Issue
Block a user