feat: Add weather awareness system (Issue #442)
Implements sensor fusion for environmental monitoring and adaptive outdoor behavior: - BME280 environmental sensor (temperature, humidity, pressure) - Phone weather API fallback (wind speed, conditions) - Camera-based rain detection (image gradient analysis) - WeatherState.msg with condition bitmask and recommendations - Behavior triggers: rain→seek shelter, wind→reduce speed, extreme temp→warning - Facial expressions: squint (rain), shiver (cold), relax (comfortable) - Real-time publishing: /saltybot/weather (WeatherState), /saltybot/weather_alert (String) - Adaptive thresholds: temp_min_safe, temp_max_safe, wind_threshold Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
This commit is contained in:
parent
b950528079
commit
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