From dc6eea40163853e9e88be7b37248a0b8c1077c30 Mon Sep 17 00:00:00 2001 From: sl-perception Date: Thu, 5 Mar 2026 09:04:20 -0500 Subject: [PATCH] feat: Add weather awareness system (Issue #442) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- jetson/ros2_ws/src/saltybot_weather/README.md | 149 +++++++ .../config/weather_params.yaml | 19 + .../saltybot_weather/launch/weather.launch.py | 90 +++++ .../ros2_ws/src/saltybot_weather/package.xml | 34 ++ .../resource/saltybot_weather | 0 .../saltybot_weather/__init__.py | 0 .../saltybot_weather/weather_node.py | 372 ++++++++++++++++++ jetson/ros2_ws/src/saltybot_weather/setup.cfg | 4 + jetson/ros2_ws/src/saltybot_weather/setup.py | 23 ++ .../src/saltybot_weather/test/__init__.py | 0 .../src/saltybot_weather/test/test_weather.py | 78 ++++ .../src/saltybot_weather_msgs/CMakeLists.txt | 16 + .../msg/WeatherState.msg | 28 ++ .../src/saltybot_weather_msgs/package.xml | 27 ++ 14 files changed, 840 insertions(+) create mode 100644 jetson/ros2_ws/src/saltybot_weather/README.md create mode 100644 jetson/ros2_ws/src/saltybot_weather/config/weather_params.yaml create mode 100644 jetson/ros2_ws/src/saltybot_weather/launch/weather.launch.py create mode 100644 jetson/ros2_ws/src/saltybot_weather/package.xml create mode 100644 jetson/ros2_ws/src/saltybot_weather/resource/saltybot_weather create mode 100644 jetson/ros2_ws/src/saltybot_weather/saltybot_weather/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_weather/saltybot_weather/weather_node.py create mode 100644 jetson/ros2_ws/src/saltybot_weather/setup.cfg create mode 100644 jetson/ros2_ws/src/saltybot_weather/setup.py create mode 100644 jetson/ros2_ws/src/saltybot_weather/test/__init__.py create mode 100644 jetson/ros2_ws/src/saltybot_weather/test/test_weather.py create mode 100644 jetson/ros2_ws/src/saltybot_weather_msgs/CMakeLists.txt create mode 100644 jetson/ros2_ws/src/saltybot_weather_msgs/msg/WeatherState.msg create mode 100644 jetson/ros2_ws/src/saltybot_weather_msgs/package.xml diff --git a/jetson/ros2_ws/src/saltybot_weather/README.md b/jetson/ros2_ws/src/saltybot_weather/README.md new file mode 100644 index 0000000..679767d --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather/README.md @@ -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 diff --git a/jetson/ros2_ws/src/saltybot_weather/config/weather_params.yaml b/jetson/ros2_ws/src/saltybot_weather/config/weather_params.yaml new file mode 100644 index 0000000..72ddaa6 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather/config/weather_params.yaml @@ -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) diff --git a/jetson/ros2_ws/src/saltybot_weather/launch/weather.launch.py b/jetson/ros2_ws/src/saltybot_weather/launch/weather.launch.py new file mode 100644 index 0000000..894a67e --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather/launch/weather.launch.py @@ -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, + ] + ) diff --git a/jetson/ros2_ws/src/saltybot_weather/package.xml b/jetson/ros2_ws/src/saltybot_weather/package.xml new file mode 100644 index 0000000..1ef1f14 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather/package.xml @@ -0,0 +1,34 @@ + + + + saltybot_weather + 0.1.0 + + 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. + + sl-perception + MIT + + ament_python + + rclpy + std_msgs + sensor_msgs + geometry_msgs + tf2_ros + saltybot_weather_msgs + saltybot_social_msgs + + python3-numpy + python3-requests + python3-opencv + + pytest + + + ament_python + + diff --git a/jetson/ros2_ws/src/saltybot_weather/resource/saltybot_weather b/jetson/ros2_ws/src/saltybot_weather/resource/saltybot_weather new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_weather/saltybot_weather/__init__.py b/jetson/ros2_ws/src/saltybot_weather/saltybot_weather/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_weather/saltybot_weather/weather_node.py b/jetson/ros2_ws/src/saltybot_weather/saltybot_weather/weather_node.py new file mode 100644 index 0000000..a4cab7a --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather/saltybot_weather/weather_node.py @@ -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() diff --git a/jetson/ros2_ws/src/saltybot_weather/setup.cfg b/jetson/ros2_ws/src/saltybot_weather/setup.cfg new file mode 100644 index 0000000..c2c5590 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/saltybot_weather +[egg_info] +tag_date = 0 diff --git a/jetson/ros2_ws/src/saltybot_weather/setup.py b/jetson/ros2_ws/src/saltybot_weather/setup.py new file mode 100644 index 0000000..960bc84 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather/setup.py @@ -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', + ], + }, +) diff --git a/jetson/ros2_ws/src/saltybot_weather/test/__init__.py b/jetson/ros2_ws/src/saltybot_weather/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/jetson/ros2_ws/src/saltybot_weather/test/test_weather.py b/jetson/ros2_ws/src/saltybot_weather/test/test_weather.py new file mode 100644 index 0000000..d9a6e7c --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather/test/test_weather.py @@ -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__]) diff --git a/jetson/ros2_ws/src/saltybot_weather_msgs/CMakeLists.txt b/jetson/ros2_ws/src/saltybot_weather_msgs/CMakeLists.txt new file mode 100644 index 0000000..084b657 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather_msgs/CMakeLists.txt @@ -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() diff --git a/jetson/ros2_ws/src/saltybot_weather_msgs/msg/WeatherState.msg b/jetson/ros2_ws/src/saltybot_weather_msgs/msg/WeatherState.msg new file mode 100644 index 0000000..65bf2cd --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather_msgs/msg/WeatherState.msg @@ -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" diff --git a/jetson/ros2_ws/src/saltybot_weather_msgs/package.xml b/jetson/ros2_ws/src/saltybot_weather_msgs/package.xml new file mode 100644 index 0000000..9439cc0 --- /dev/null +++ b/jetson/ros2_ws/src/saltybot_weather_msgs/package.xml @@ -0,0 +1,27 @@ + + + + saltybot_weather_msgs + 0.1.0 + + Custom ROS2 message definitions for weather awareness and environmental sensing. + Includes WeatherState for sensor fusion (BME280 + phone API + camera rain detection). + Issue #442. + + sl-perception + MIT + + ament_cmake + rosidl_default_generators + + std_msgs + builtin_interfaces + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + +