From d62115d382527f4c102eb79da85012efc2bf4f1b Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Thu, 4 Jun 2026 14:11:39 +0200 Subject: [PATCH 01/18] added weather service node. Signed-off-by: Andrew Koerner --- .../weather_service_interface/.gitignore | 8 + .../weather_service_interface/Dockerfile | 28 ++ .../weather_service_interface/Makefile | 106 +++++ .../weather_service_interface/README.md | 92 +++++ .../weather_service_interface/__init__.py | 0 .../config/weather_service_config.yaml | 55 +++ .../weather_service_interface/forecast.py | 55 +++ .../forecast_logger.py | 39 ++ .../launch/weather_service.launch.py | 25 ++ .../weather_service_and_visualizer.launch.py | 62 +++ .../weather_service_interface/package.xml | 19 + .../requirements.pip3 | 3 + .../requirements.system | 1 + .../resource/weather_service_interface | 0 .../weather_service_interface/setup.cfg | 8 + .../weather_service_interface/setup.py | 31 ++ .../weather_service_interface/sources.py | 251 ++++++++++++ .../start_visualizer.sh | 29 ++ .../start_weather_service.sh | 5 + .../start_weather_service_and_visualizer.sh | 16 + .../weather_service_config.yaml | 1 + .../weather_service_interface/__init__.py | 0 .../weather_service_interface/forecast.py | 55 +++ .../forecast_logger.py | 39 ++ .../weather_service_interface/map_panel.py | 278 ++++++++++++++ .../weather_service_interface/sources.py | 251 ++++++++++++ .../weather_service_node.py | 221 +++++++++++ .../weather_visualizer_node.py | 363 ++++++++++++++++++ .../weather_service_node.py | 221 +++++++++++ .../weather_visualizer_node.py | 319 +++++++++++++++ 30 files changed, 2581 insertions(+) create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/.gitignore create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/Dockerfile create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/Makefile create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/README.md create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/__init__.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/config/weather_service_config.yaml create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/forecast.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/forecast_logger.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/launch/weather_service.launch.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/launch/weather_service_and_visualizer.launch.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/package.xml create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/requirements.pip3 create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/requirements.system create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/resource/weather_service_interface create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/setup.cfg create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/setup.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/sources.py create mode 100755 ros2_workspace/src/adore_interfaces/weather_service_interface/start_visualizer.sh create mode 100755 ros2_workspace/src/adore_interfaces/weather_service_interface/start_weather_service.sh create mode 100755 ros2_workspace/src/adore_interfaces/weather_service_interface/start_weather_service_and_visualizer.sh create mode 120000 ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_config.yaml create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/__init__.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/forecast.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/forecast_logger.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/map_panel.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/sources.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/weather_service_node.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/weather_visualizer_node.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_node.py create mode 100644 ros2_workspace/src/adore_interfaces/weather_service_interface/weather_visualizer_node.py diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/.gitignore b/ros2_workspace/src/adore_interfaces/weather_service_interface/.gitignore new file mode 100644 index 00000000..dad48fc7 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/.gitignore @@ -0,0 +1,8 @@ +build/ +install/ +log/ +__pycache__/ +*.pyc +*.egg-info/ +.eggs/ +dist/ diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/Dockerfile b/ros2_workspace/src/adore_interfaces/weather_service_interface/Dockerfile new file mode 100644 index 00000000..f4d278ce --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/Dockerfile @@ -0,0 +1,28 @@ +ARG ROS_DISTRO=jazzy +ARG OS_CODE_NAME=noble +FROM ros:${ROS_DISTRO}-ros-core-${OS_CODE_NAME} AS base + +ARG ROS_DISTRO=jazzy +ENV ROS_DISTRO=${ROS_DISTRO} +ENV DEBIAN_FRONTEND=noninteractive + +COPY requirements.system /tmp/requirements.system +RUN apt-get update && apt-get install -y \ + python3-pip \ + python3-colcon-common-extensions \ + $(grep -v '^\s*#' /tmp/requirements.system | tr '\n' ' ') \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /ros2_ws/src/weather_service_interface +COPY . . + +RUN pip3 install --break-system-packages -r requirements.pip3 + +WORKDIR /ros2_ws +RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \ + colcon build --packages-select weather_service_interface + +RUN printf '#!/usr/bin/env bash\nset -e\nsource /opt/ros/${ROS_DISTRO}/setup.bash\nsource /ros2_ws/install/setup.bash\nexec "$@"\n' > /entrypoint.sh && \ + chmod +x /entrypoint.sh + +ENTRYPOINT ["/entrypoint.sh"] diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/Makefile b/ros2_workspace/src/adore_interfaces/weather_service_interface/Makefile new file mode 100644 index 00000000..33411601 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/Makefile @@ -0,0 +1,106 @@ +.DEFAULT_GOAL := help + +IMAGE_NAME := weather_service_interface +IMAGE_TAG := latest +ROS_DISTRO := jazzy +OS_CODE_NAME := noble +SAVE_FILE := $(IMAGE_NAME).tar + +CONFIG := $(shell pwd)/config/weather_service_config.yaml +ROS_DOMAIN_ID := 0 +ROS_HOME := $(HOME)/.ros + +DISPLAY ?= $(DISPLAY) +XAUTH ?= $(XAUTHORITY) +X11_SOCKET := /tmp/.X11-unix + +DOCKER_COMMON := docker run --rm \ + --network host \ + --env ROS_DOMAIN_ID=$(ROS_DOMAIN_ID) \ + --env ROS_HOME=/ros_home \ + --env FASTDDS_BUILTIN_TRANSPORTS=UDPv4 \ + --volume $(ROS_HOME):/ros_home \ + --volume $(CONFIG):/config/weather_service_config.yaml:ro + +DOCKER_DISPLAY := --env DISPLAY=$(DISPLAY) \ + --env WEATHER_CONFIG_PATH=/config/weather_service_config.yaml \ + $(if $(XAUTH),--env XAUTHORITY=/tmp/.Xauthority --volume $(XAUTH):/tmp/.Xauthority:ro) \ + $(if $(wildcard $(X11_SOCKET)),--volume $(X11_SOCKET):$(X11_SOCKET)) + +DOCKER_RUN := $(DOCKER_COMMON) \ + $(IMAGE_NAME):$(IMAGE_TAG) + +.PHONY: help +help: + @echo "weather_service_interface" + @echo "" + @echo "Usage: make " + @echo "" + @echo "Targets:" + @echo " build Build the Docker image" + @echo " run Run the weather service node" + @echo " run_visualizer Run the weather service + visualizer (service logs to /tmp/weather_service.log)" + @echo " shell Open a shell inside the container" + @echo " save Save the Docker image to $(SAVE_FILE)" + @echo " load Load the Docker image from $(SAVE_FILE)" + @echo " clean Remove the Docker image and saved file" + @echo "" + @echo "Variables (override on the command line):" + @echo " CONFIG Path to service config (default: config/weather_service_config.yaml)" + @echo " ROS_DOMAIN_ID ROS domain ID (default: $(ROS_DOMAIN_ID))" + @echo " ROS_HOME Host path for forecast logs (default: $(ROS_HOME))" + @echo " DISPLAY X11 display for xterm (default: \$$DISPLAY)" + @echo " XAUTH Xauthority file for xterm (default: \$$XAUTHORITY)" + @echo " ROS_DISTRO ROS distribution (default: $(ROS_DISTRO))" + @echo " OS_CODE_NAME Ubuntu codename (default: $(OS_CODE_NAME))" + @echo " IMAGE_TAG Docker image tag (default: $(IMAGE_TAG))" + +.PHONY: build +build: + docker build \ + --build-arg ROS_DISTRO=$(ROS_DISTRO) \ + --build-arg OS_CODE_NAME=$(OS_CODE_NAME) \ + -t $(IMAGE_NAME):$(IMAGE_TAG) . + +.PHONY: run +run: + $(DOCKER_RUN) ros2 launch weather_service_interface weather_service.launch.py \ + config_path:=/config/weather_service_config.yaml + +# Service output is redirected to /tmp/weather_service.log so it does not +# corrupt the curses display. DOCKER_DISPLAY forwards X11 sockets and auth +# for xterm when visualizer.use_xterm is enabled in the config. +.PHONY: run_visualizer +run_visualizer: + @$(DOCKER_RUN) ros2 launch weather_service_interface weather_service.launch.py \ + config_path:=/config/weather_service_config.yaml \ + >/tmp/weather_service.log 2>&1 & \ + SERVICE_PID=$$!; \ + trap "kill $$SERVICE_PID 2>/dev/null; wait $$SERVICE_PID 2>/dev/null" EXIT INT TERM; \ + docker run --rm -it \ + --network host \ + --env ROS_DOMAIN_ID=$(ROS_DOMAIN_ID) \ + --env ROS_HOME=/ros_home \ + --env FASTDDS_BUILTIN_TRANSPORTS=UDPv4 \ + --volume $(ROS_HOME):/ros_home \ + $(DOCKER_DISPLAY) \ + --volume $(CONFIG):/config/weather_service_config.yaml:ro \ + $(IMAGE_NAME):$(IMAGE_TAG) \ + ros2 run weather_service_interface weather_visualizer_node + +.PHONY: shell +shell: + $(DOCKER_RUN) bash + +.PHONY: save +save: + docker save $(IMAGE_NAME):$(IMAGE_TAG) -o $(SAVE_FILE) + +.PHONY: load +load: + docker load -i $(SAVE_FILE) + +.PHONY: clean +clean: + docker rmi $(IMAGE_NAME):$(IMAGE_TAG) || true + rm -f $(SAVE_FILE) diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/README.md b/ros2_workspace/src/adore_interfaces/weather_service_interface/README.md new file mode 100644 index 00000000..12f83ef3 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/README.md @@ -0,0 +1,92 @@ +# weather_service_interface + +ROS 2 weather forecast service for autonomous vehicles. Fetches forecasts from external weather APIs based on ego vehicle position and publishes them at low frequency. + +## Nodes + +### `weather_service_node` + +Subscribes to the ego vehicle GPS position and fetches weather data when the vehicle moves beyond a configurable displacement threshold or the cached forecast expires. Publishes forecast JSON to `/ego_vehicle/weather_forecast` at a configurable low frequency (default 0.1 Hz). + +**Subscribed topics** +- `/ego_vehicle/vehicle_state_dynamic_nav_sat_fix` (`sensor_msgs/NavSatFix`) + +**Published topics** +- `/ego_vehicle/weather_forecast` (`std_msgs/String` — JSON-serialized forecast) + +**Forecast logs** are written to `$ROS_HOME/weather_forecasts/` on each successful fetch. + +### `weather_visualizer_node` _(optional)_ + +Terminal-based curses dashboard showing the next 12 hours of forecast data. Must be run in a separate terminal with a real TTY — it cannot be a child of `ros2 launch`. + +## Data sources + +Sources are tried in priority order. Set `enabled: false` in config to skip one. + +| Priority | Source | Coverage | API key | +|----------|--------|----------|---------| +| 1 | DWD via Brightsky (`api.brightsky.dev`) | Germany / Europe | None | +| 2 | NOAA (`api.weather.gov`) | US only | None | +| 3 | Open-Meteo (`api.open-meteo.com`) | Global (fallback) | None | + +## Configuration + +`config/weather_service_config.yaml` + +```yaml +fetch: + displacement_threshold_m: 5000.0 # minimum vehicle movement before re-fetch + max_forecast_age_s: 3600.0 # force re-fetch after this age + check_interval_s: 60.0 # how often to check if fetch is needed + publish_interval_s: 10.0 # forecast publish rate (0.1 Hz default) + +sources: + noaa: { enabled: true, priority: 1 } + dwd: { enabled: true, priority: 2 } + open_meteo: { enabled: true, priority: 3 } + +logging: + enabled: true + max_files: 100 +``` + +## Usage + +### Without Docker + +```bash +# build +colcon build --packages-select weather_service_interface + +# service only +./start_weather_service.sh + +# service + visualizer (separate terminals) +./start_weather_service.sh +./start_visualizer.sh +``` + +### With Docker + +```bash +make build + +make run # service only +make run_visualizer # service in background + visualizer with TTY +make shell # interactive shell in container +``` + +Override config or ROS domain on the command line: + +```bash +make run CONFIG=/path/to/custom_config.yaml ROS_DOMAIN_ID=42 +``` + +Service logs are written to `/tmp/weather_service.log` when running via `run_visualizer`. Forecast JSON files land in `$ROS_HOME/weather_forecasts/` on the host via the bind mount. + +## Visualizer keys + +| Key | Action | +|-----|--------| +| `q` / `ESC` | Quit | diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/__init__.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/config/weather_service_config.yaml b/ros2_workspace/src/adore_interfaces/weather_service_interface/config/weather_service_config.yaml new file mode 100644 index 00000000..232698d4 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/config/weather_service_config.yaml @@ -0,0 +1,55 @@ +# ─── Fetch policy ──────────────────────────────────────────────────────────── +fetch: + # Minimum distance (metres) the vehicle must move before a new fetch is triggered. + displacement_threshold_m: 5000.0 + + # Maximum age (seconds) of a cached forecast before it is considered stale. + max_forecast_age_s: 3600.0 + + # How often the service node checks whether a fetch is needed (seconds). + # This does NOT control how often external APIs are called. + check_interval_s: 60.0 + + # How often the latest forecast is re-published regardless of updates (seconds). + # Keep this low-frequency; 0.1 Hz (every 10 s) is a reasonable default. + publish_interval_s: 10.0 + +# ─── Data sources ──────────────────────────────────────────────────────────── +# Sources are tried in priority order (lowest number first). +# Set enabled: false to skip a source entirely. +sources: + noaa: + enabled: true + priority: 2 + # Timeout for each HTTP request in seconds. + timeout_s: 10.0 + + dwd: + enabled: true + priority: 1 + timeout_s: 10.0 + + # Open-Meteo is the fallback: no API key required, global coverage. + open_meteo: + enabled: true + priority: 3 + timeout_s: 10.0 + +# ─── Logging ───────────────────────────────────────────────────────────────── +logging: + # Write each forecast JSON to $ROS_HOME/weather_forecasts/. + enabled: true + # Maximum number of forecast files to retain (oldest are pruned). + max_files: 100 + +# ─── Visualizer ────────────────────────────────────────────────────────────── +visualizer: + # Launch the visualizer in a dedicated xterm window. + # Falls back to the current terminal session if xterm is not installed. + use_xterm: true + +# ─── Topics ────────────────────────────────────────────────────────────────── +topics: + nav_sat_fix: /ego_vehicle/vehicle_state_dynamic_nav_sat_fix + weather_forecast: /ego_vehicle/weather_forecast + queue_depth: 10 diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/forecast.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/forecast.py new file mode 100644 index 00000000..8aad90f8 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/forecast.py @@ -0,0 +1,55 @@ +from __future__ import annotations + +import time +from dataclasses import dataclass, field, asdict +from typing import Optional + + +@dataclass +class HourlyEntry: + time_iso: str + temperature_c: float + apparent_temperature_c: float + precipitation_mm: float + precipitation_probability_pct: float + wind_speed_kmh: float + wind_direction_deg: float + wind_gusts_kmh: float + cloud_cover_pct: float + visibility_m: float + weather_code: int + weather_description: str + + +@dataclass +class WeatherForecast: + source: str + latitude: float + longitude: float + fetch_time_unix: float + valid_until_unix: float + timezone: str + hourly: list[HourlyEntry] = field(default_factory=list) + + def is_expired(self, max_age_s: float) -> bool: + return (time.time() - self.fetch_time_unix) > max_age_s + + def to_dict(self) -> dict: + return asdict(self) + + @staticmethod + def weather_code_description(code: int) -> str: + _WMO = { + 0: 'Clear sky', + 1: 'Mainly clear', 2: 'Partly cloudy', 3: 'Overcast', + 45: 'Fog', 48: 'Depositing rime fog', + 51: 'Light drizzle', 53: 'Moderate drizzle', 55: 'Dense drizzle', + 61: 'Slight rain', 63: 'Moderate rain', 65: 'Heavy rain', + 71: 'Slight snow', 73: 'Moderate snow', 75: 'Heavy snow', + 77: 'Snow grains', + 80: 'Slight showers', 81: 'Moderate showers', 82: 'Violent showers', + 85: 'Slight snow showers', 86: 'Heavy snow showers', + 95: 'Thunderstorm', 96: 'Thunderstorm with slight hail', + 99: 'Thunderstorm with heavy hail', + } + return _WMO.get(code, f'Unknown ({code})') diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/forecast_logger.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/forecast_logger.py new file mode 100644 index 00000000..02797a0f --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/forecast_logger.py @@ -0,0 +1,39 @@ +from __future__ import annotations + +import json +import os +import time +from pathlib import Path + +from .forecast import WeatherForecast + + +class ForecastLogger: + def __init__(self, enabled: bool, max_files: int = 100): + self._enabled = enabled + self._max_files = max_files + self._log_dir = self._resolve_log_dir() + if self._enabled: + self._log_dir.mkdir(parents=True, exist_ok=True) + + @staticmethod + def _resolve_log_dir() -> Path: + ros_home = os.environ.get('ROS_HOME', os.path.expanduser('~/.ros')) + return Path(ros_home) / 'weather_forecasts' + + def log(self, forecast: WeatherForecast) -> None: + if not self._enabled: + return + + ts = int(forecast.fetch_time_unix) + filename = self._log_dir / f'forecast_{ts}_{forecast.source}.json' + with open(filename, 'w') as f: + json.dump(forecast.to_dict(), f, indent=2) + + self._prune() + + def _prune(self) -> None: + files = sorted(self._log_dir.glob('forecast_*.json'), key=lambda p: p.stat().st_mtime) + excess = len(files) - self._max_files + for f in files[:excess]: + f.unlink(missing_ok=True) diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/launch/weather_service.launch.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/launch/weather_service.launch.py new file mode 100644 index 00000000..9302d336 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/launch/weather_service.launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +import os + + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('weather_service_interface'), + 'config', 'weather_service_config.yaml', + ) + + return LaunchDescription([ + DeclareLaunchArgument('config_path', default_value=config), + + Node( + package='weather_service_interface', + executable='weather_service_node', + name='weather_service_node', + parameters=[{'config_path': LaunchConfiguration('config_path')}], + output='screen', + ), + ]) diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/launch/weather_service_and_visualizer.launch.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/launch/weather_service_and_visualizer.launch.py new file mode 100644 index 00000000..a34fc90b --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/launch/weather_service_and_visualizer.launch.py @@ -0,0 +1,62 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +import os +import yaml + + +def _use_xterm(config_path: str) -> bool: + try: + with open(config_path) as f: + cfg = yaml.safe_load(f) + return bool(cfg.get('visualizer', {}).get('use_xterm', False)) + except Exception: + return False + + +def _xterm_available() -> bool: + import shutil + return shutil.which('xterm') is not None + + +def generate_launch_description(): + default_config = os.path.join( + get_package_share_directory('weather_service_interface'), + 'config', 'weather_service_config.yaml', + ) + + config_path = LaunchConfiguration('config_path') + + # Resolve the config at launch-description-generation time so we can read + # the visualizer.use_xterm flag. LaunchConfiguration values are not yet + # substituted here, so we read the default directly; if the caller passes + # config_path on the command line we honour that too via the env-var path. + resolved_config = os.environ.get('WEATHER_CONFIG_PATH', default_config) + + use_xterm = _use_xterm(resolved_config) and _xterm_available() + visualizer_prefix = 'xterm -e' if use_xterm else '' + + return LaunchDescription([ + DeclareLaunchArgument('config_path', default_value=default_config), + + Node( + package='weather_service_interface', + executable='weather_service_node', + name='weather_service_node', + parameters=[{'config_path': config_path}], + output='screen', + ), + + # The visualizer owns a TTY for curses. When use_xterm is enabled it + # runs in its own xterm window; otherwise it takes the current terminal. + Node( + package='weather_service_interface', + executable='weather_visualizer_node', + name='weather_visualizer_node', + parameters=[{'config_path': config_path}], + prefix=visualizer_prefix, + output='screen', + ), + ]) diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/package.xml b/ros2_workspace/src/adore_interfaces/weather_service_interface/package.xml new file mode 100644 index 00000000..253580d6 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/package.xml @@ -0,0 +1,19 @@ + + + weather_service_interface + 1.0.0 + ROS 2 weather forecast service node: fetches forecasts from NOAA, DWD, or Open-Meteo and publishes to /ego_vehicle/weather_forecast + maintainer + Apache-2.0 + rclpy + sensor_msgs + std_msgs + python3-requests + python3-yaml + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_python + + diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/requirements.pip3 b/ros2_workspace/src/adore_interfaces/weather_service_interface/requirements.pip3 new file mode 100644 index 00000000..e7f265cf --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/requirements.pip3 @@ -0,0 +1,3 @@ +requests>=2.28.0 +pyyaml>=6.0 +pillow>=10.0.0 diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/requirements.system b/ros2_workspace/src/adore_interfaces/weather_service_interface/requirements.system new file mode 100644 index 00000000..db0c341f --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/requirements.system @@ -0,0 +1 @@ +xterm diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/resource/weather_service_interface b/ros2_workspace/src/adore_interfaces/weather_service_interface/resource/weather_service_interface new file mode 100644 index 00000000..e69de29b diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/setup.cfg b/ros2_workspace/src/adore_interfaces/weather_service_interface/setup.cfg new file mode 100644 index 00000000..389f5648 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/setup.cfg @@ -0,0 +1,8 @@ +[metadata] +name = weather_service_interface + +[develop] +script_dir=$base/lib/weather_service_interface + +[install] +install_scripts=$base/lib/weather_service_interface diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/setup.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/setup.py new file mode 100644 index 00000000..dcedd5d4 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/setup.py @@ -0,0 +1,31 @@ +from setuptools import setup + +package_name = 'weather_service_interface' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/config', [ + 'config/weather_service_config.yaml', + ]), + ('share/' + package_name + '/launch', [ + 'launch/weather_service.launch.py', + 'launch/weather_service_and_visualizer.launch.py', + ]), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='maintainer', + description='ROS 2 weather forecast service interface', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'weather_service_node = weather_service_interface.weather_service_node:main', + 'weather_visualizer_node = weather_service_interface.weather_visualizer_node:main', + ], + }, +) diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/sources.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/sources.py new file mode 100644 index 00000000..cc8e0520 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/sources.py @@ -0,0 +1,251 @@ +from __future__ import annotations + +import time +import logging +from abc import ABC, abstractmethod +from typing import Optional + +import requests + +from .forecast import WeatherForecast, HourlyEntry + +log = logging.getLogger(__name__) + + +class WeatherSource(ABC): + name: str + + @abstractmethod + def fetch(self, lat: float, lon: float, timeout_s: float) -> Optional[WeatherForecast]: + ... + + +class NOAASource(WeatherSource): + name = 'noaa' + + _POINTS_URL = 'https://api.weather.gov/points/{lat},{lon}' + _HEADERS = {'User-Agent': 'weather_service_interface/1.0 (ros2-node)'} + + def fetch(self, lat: float, lon: float, timeout_s: float) -> Optional[WeatherForecast]: + try: + points_url = self._POINTS_URL.format(lat=round(lat, 4), lon=round(lon, 4)) + r = requests.get(points_url, headers=self._HEADERS, timeout=timeout_s) + if r.status_code == 404: + log.debug('NOAA: location outside coverage (%.4f, %.4f)', lat, lon) + return None + r.raise_for_status() + props = r.json()['properties'] + forecast_hourly_url = props['forecastHourly'] + + r2 = requests.get(forecast_hourly_url, headers=self._HEADERS, timeout=timeout_s) + r2.raise_for_status() + periods = r2.json()['properties']['periods'] + + hourly: list[HourlyEntry] = [] + for p in periods[:48]: + wind_spd_raw: str = p.get('windSpeed', '0 mph') + try: + wind_kmh = float(wind_spd_raw.split()[0]) * 1.60934 + except (ValueError, IndexError): + wind_kmh = 0.0 + + temp_c = (p['temperature'] - 32) * 5 / 9 if p.get('temperatureUnit') == 'F' else float(p['temperature']) + + hourly.append(HourlyEntry( + time_iso=p['startTime'], + temperature_c=round(temp_c, 1), + apparent_temperature_c=round(temp_c, 1), + precipitation_mm=0.0, + precipitation_probability_pct=float(p.get('probabilityOfPrecipitation', {}).get('value') or 0), + wind_speed_kmh=round(wind_kmh, 1), + wind_direction_deg=0.0, + wind_gusts_kmh=0.0, + cloud_cover_pct=0.0, + visibility_m=0.0, + weather_code=0, + weather_description=p.get('shortForecast', ''), + )) + + now = time.time() + return WeatherForecast( + source='noaa', + latitude=lat, + longitude=lon, + fetch_time_unix=now, + valid_until_unix=now + 3600 * 48, + timezone='UTC', + hourly=hourly, + ) + except requests.RequestException as e: + log.warning('NOAA fetch failed: %s', e) + return None + except Exception as e: + log.warning('NOAA parse error: %s', e) + return None + + +class DWDSource(WeatherSource): + """Deutscher Wetterdienst via Brightsky API. + + Default Brightsky units (no units param): temperature °C, wind km/h, + precipitation mm, precipitation_probability 0-100 integer. + """ + name = 'dwd' + + _URL = 'https://api.brightsky.dev/weather' + + def fetch(self, lat: float, lon: float, timeout_s: float) -> Optional[WeatherForecast]: + try: + from datetime import datetime, timezone, timedelta + now_utc = datetime.now(timezone.utc) + date_from = now_utc.strftime('%Y-%m-%dT%H:%M:%S') + date_to = (now_utc + timedelta(hours=48)).strftime('%Y-%m-%dT%H:%M:%S') + + params = { + 'lat': lat, + 'lon': lon, + 'date': date_from, + 'last_date': date_to, + } + r = requests.get(self._URL, params=params, timeout=timeout_s) + r.raise_for_status() + data = r.json() + + hourly: list[HourlyEntry] = [] + for entry in data.get('weather', []): + condition = entry.get('condition', '') + icon = entry.get('icon', '') + hourly.append(HourlyEntry( + time_iso=entry.get('timestamp', ''), + temperature_c=float(entry.get('temperature') or 0), + apparent_temperature_c=float(entry.get('temperature') or 0), + precipitation_mm=float(entry.get('precipitation') or 0), + precipitation_probability_pct=float(entry.get('precipitation_probability') or 0), + wind_speed_kmh=float(entry.get('wind_speed') or 0), + wind_direction_deg=float(entry.get('wind_direction') or 0), + wind_gusts_kmh=float(entry.get('wind_gust_speed') or 0), + cloud_cover_pct=float(entry.get('cloud_cover') or 0), + visibility_m=float(entry.get('visibility') or 0), + weather_code=0, + weather_description=condition or icon, + )) + + now_ts = time.time() + return WeatherForecast( + source='dwd', + latitude=lat, + longitude=lon, + fetch_time_unix=now_ts, + valid_until_unix=now_ts + 3600 * 48, + timezone=data.get('sources', [{}])[0].get('timezone', 'UTC'), + hourly=hourly, + ) + except requests.RequestException as e: + log.warning('DWD fetch failed: %s', e) + return None + except Exception as e: + log.warning('DWD parse error: %s', e) + return None + + +class OpenMeteoSource(WeatherSource): + """Open-Meteo: free, no API key, global coverage. Used as fallback.""" + name = 'open_meteo' + + _URL = 'https://api.open-meteo.com/v1/forecast' + + _HOURLY_VARS = [ + 'temperature_2m', + 'apparent_temperature', + 'precipitation', + 'precipitation_probability', + 'wind_speed_10m', + 'wind_direction_10m', + 'wind_gusts_10m', + 'cloud_cover', + 'visibility', + 'weather_code', + ] + + def fetch(self, lat: float, lon: float, timeout_s: float) -> Optional[WeatherForecast]: + try: + params = { + 'latitude': lat, + 'longitude': lon, + 'hourly': ','.join(self._HOURLY_VARS), + 'forecast_days': 2, + 'wind_speed_unit': 'kmh', + 'timezone': 'UTC', + } + r = requests.get(self._URL, params=params, timeout=timeout_s) + r.raise_for_status() + data = r.json() + + h = data.get('hourly', {}) + times = h.get('time', []) + n = len(times) + + def _col(key: str) -> list: + vals = h.get(key, [None] * n) + return [v if v is not None else 0 for v in vals] + + temps = _col('temperature_2m') + feels = _col('apparent_temperature') + precip = _col('precipitation') + precip_p = _col('precipitation_probability') + wspd = _col('wind_speed_10m') + wdir = _col('wind_direction_10m') + wgust = _col('wind_gusts_10m') + cloud = _col('cloud_cover') + vis = _col('visibility') + wcodes = _col('weather_code') + + hourly: list[HourlyEntry] = [] + for i in range(n): + code = int(wcodes[i]) + hourly.append(HourlyEntry( + time_iso=times[i], + temperature_c=float(temps[i]), + apparent_temperature_c=float(feels[i]), + precipitation_mm=float(precip[i]), + precipitation_probability_pct=float(precip_p[i]), + wind_speed_kmh=float(wspd[i]), + wind_direction_deg=float(wdir[i]), + wind_gusts_kmh=float(wgust[i]), + cloud_cover_pct=float(cloud[i]), + visibility_m=float(vis[i]), + weather_code=code, + weather_description=WeatherForecast.weather_code_description(code), + )) + + now = time.time() + return WeatherForecast( + source='open_meteo', + latitude=data.get('latitude', lat), + longitude=data.get('longitude', lon), + fetch_time_unix=now, + valid_until_unix=now + 3600 * 48, + timezone=data.get('timezone', 'UTC'), + hourly=hourly, + ) + except requests.RequestException as e: + log.warning('Open-Meteo fetch failed: %s', e) + return None + except Exception as e: + log.warning('Open-Meteo parse error: %s', e) + return None + + +def build_source_chain(sources_cfg: dict) -> list[WeatherSource]: + registry: dict[str, WeatherSource] = { + 'noaa': NOAASource(), + 'dwd': DWDSource(), + 'open_meteo': OpenMeteoSource(), + } + enabled = [ + (name, src) + for name, src in registry.items() + if sources_cfg.get(name, {}).get('enabled', True) + ] + enabled.sort(key=lambda x: sources_cfg.get(x[0], {}).get('priority', 99)) + return [src for _, src in enabled] diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/start_visualizer.sh b/ros2_workspace/src/adore_interfaces/weather_service_interface/start_visualizer.sh new file mode 100755 index 00000000..1e3ada0f --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/start_visualizer.sh @@ -0,0 +1,29 @@ +#!/usr/bin/env bash +set -e +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +CONFIG="${1:-$SCRIPT_DIR/config/weather_service_config.yaml}" + +USE_XTERM=false +if command -v python3 &>/dev/null && [ -f "$CONFIG" ]; then + USE_XTERM=$(python3 -c " +import sys, yaml +try: + cfg = yaml.safe_load(open('$CONFIG')) + print(str(cfg.get('visualizer', {}).get('use_xterm', False)).lower()) +except Exception: + print('false') +") +fi + +VISUALIZER_CMD="ros2 run weather_service_interface weather_visualizer_node" + +if [ "$USE_XTERM" = "true" ]; then + if command -v xterm &>/dev/null; then + exec xterm -e "$VISUALIZER_CMD" + else + echo "warning: use_xterm is set but xterm is not installed, falling back to current session" >&2 + exec $VISUALIZER_CMD + fi +else + exec $VISUALIZER_CMD +fi diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/start_weather_service.sh b/ros2_workspace/src/adore_interfaces/weather_service_interface/start_weather_service.sh new file mode 100755 index 00000000..b866e024 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/start_weather_service.sh @@ -0,0 +1,5 @@ +#!/usr/bin/env bash +set -e +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +CONFIG="${1:-$SCRIPT_DIR/config/weather_service_config.yaml}" +ros2 launch weather_service_interface weather_service.launch.py config_path:="$CONFIG" diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/start_weather_service_and_visualizer.sh b/ros2_workspace/src/adore_interfaces/weather_service_interface/start_weather_service_and_visualizer.sh new file mode 100755 index 00000000..fb3bdb31 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/start_weather_service_and_visualizer.sh @@ -0,0 +1,16 @@ +#!/usr/bin/env bash +set -e +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +CONFIG="${1:-$SCRIPT_DIR/config/weather_service_config.yaml}" + +ros2 launch weather_service_interface weather_service.launch.py config_path:="$CONFIG" \ + > /tmp/weather_service.log 2>&1 & +SERVICE_PID=$! + +cleanup() { + kill "$SERVICE_PID" 2>/dev/null || true + wait "$SERVICE_PID" 2>/dev/null || true +} +trap cleanup EXIT INT TERM + +"$SCRIPT_DIR/start_visualizer.sh" "$CONFIG" diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_config.yaml b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_config.yaml new file mode 120000 index 00000000..a9118fee --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_config.yaml @@ -0,0 +1 @@ +config/weather_service_config.yaml \ No newline at end of file diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/__init__.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/forecast.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/forecast.py new file mode 100644 index 00000000..8aad90f8 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/forecast.py @@ -0,0 +1,55 @@ +from __future__ import annotations + +import time +from dataclasses import dataclass, field, asdict +from typing import Optional + + +@dataclass +class HourlyEntry: + time_iso: str + temperature_c: float + apparent_temperature_c: float + precipitation_mm: float + precipitation_probability_pct: float + wind_speed_kmh: float + wind_direction_deg: float + wind_gusts_kmh: float + cloud_cover_pct: float + visibility_m: float + weather_code: int + weather_description: str + + +@dataclass +class WeatherForecast: + source: str + latitude: float + longitude: float + fetch_time_unix: float + valid_until_unix: float + timezone: str + hourly: list[HourlyEntry] = field(default_factory=list) + + def is_expired(self, max_age_s: float) -> bool: + return (time.time() - self.fetch_time_unix) > max_age_s + + def to_dict(self) -> dict: + return asdict(self) + + @staticmethod + def weather_code_description(code: int) -> str: + _WMO = { + 0: 'Clear sky', + 1: 'Mainly clear', 2: 'Partly cloudy', 3: 'Overcast', + 45: 'Fog', 48: 'Depositing rime fog', + 51: 'Light drizzle', 53: 'Moderate drizzle', 55: 'Dense drizzle', + 61: 'Slight rain', 63: 'Moderate rain', 65: 'Heavy rain', + 71: 'Slight snow', 73: 'Moderate snow', 75: 'Heavy snow', + 77: 'Snow grains', + 80: 'Slight showers', 81: 'Moderate showers', 82: 'Violent showers', + 85: 'Slight snow showers', 86: 'Heavy snow showers', + 95: 'Thunderstorm', 96: 'Thunderstorm with slight hail', + 99: 'Thunderstorm with heavy hail', + } + return _WMO.get(code, f'Unknown ({code})') diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/forecast_logger.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/forecast_logger.py new file mode 100644 index 00000000..02797a0f --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/forecast_logger.py @@ -0,0 +1,39 @@ +from __future__ import annotations + +import json +import os +import time +from pathlib import Path + +from .forecast import WeatherForecast + + +class ForecastLogger: + def __init__(self, enabled: bool, max_files: int = 100): + self._enabled = enabled + self._max_files = max_files + self._log_dir = self._resolve_log_dir() + if self._enabled: + self._log_dir.mkdir(parents=True, exist_ok=True) + + @staticmethod + def _resolve_log_dir() -> Path: + ros_home = os.environ.get('ROS_HOME', os.path.expanduser('~/.ros')) + return Path(ros_home) / 'weather_forecasts' + + def log(self, forecast: WeatherForecast) -> None: + if not self._enabled: + return + + ts = int(forecast.fetch_time_unix) + filename = self._log_dir / f'forecast_{ts}_{forecast.source}.json' + with open(filename, 'w') as f: + json.dump(forecast.to_dict(), f, indent=2) + + self._prune() + + def _prune(self) -> None: + files = sorted(self._log_dir.glob('forecast_*.json'), key=lambda p: p.stat().st_mtime) + excess = len(files) - self._max_files + for f in files[:excess]: + f.unlink(missing_ok=True) diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/map_panel.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/map_panel.py new file mode 100644 index 00000000..ae14b901 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/map_panel.py @@ -0,0 +1,278 @@ +from __future__ import annotations + +import io +import math +import threading +import time +from typing import Optional + +import requests +from PIL import Image + +# ── OSM tile helpers ────────────────────────────────────────────────────────── + +_TILE_URL = 'https://tile.openstreetmap.org/{z}/{x}/{y}.png' +_TILE_SIZE = 256 +_HEADERS = {'User-Agent': 'weather_service_interface/1.0 (ros2-node; map-panel)'} + +_tile_cache: dict[tuple[int, int, int], Optional[Image.Image]] = {} +_tile_lock = threading.Lock() + + +def _deg_to_tile(lat: float, lon: float, zoom: int) -> tuple[int, int]: + n = 1 << zoom + x = int((lon + 180.0) / 360.0 * n) + lat_r = math.radians(lat) + y = int((1.0 - math.log(math.tan(lat_r) + 1.0 / math.cos(lat_r)) / math.pi) / 2.0 * n) + return x, y + + +def _tile_origin_deg(tx: int, ty: int, zoom: int) -> tuple[float, float]: + """Return (lat, lon) of the NW corner of tile (tx, ty).""" + n = 1 << zoom + lon = tx / n * 360.0 - 180.0 + lat = math.degrees(math.atan(math.sinh(math.pi * (1 - 2 * ty / n)))) + return lat, lon + + +def _fetch_tile(z: int, x: int, y: int) -> Optional[Image.Image]: + key = (z, x, y) + with _tile_lock: + if key in _tile_cache: + return _tile_cache[key] + + try: + r = requests.get( + _TILE_URL.format(z=z, x=x, y=y), + headers=_HEADERS, + timeout=5.0, + ) + r.raise_for_status() + img = Image.open(io.BytesIO(r.content)).convert('L') + except Exception: + img = None + + with _tile_lock: + _tile_cache[key] = img + + return img + + +# ── braille rendering ───────────────────────────────────────────────────────── + +# Braille block is 2 cols × 4 rows of dots. +# Unicode U+2800 is the empty braille cell; each dot adds a fixed offset. +# dot layout (col, row) -> bit +# (0,0)->0 (1,0)->3 +# (0,1)->1 (1,1)->4 +# (0,2)->2 (1,2)->5 +# (0,3)->6 (1,3)->7 +_DOT_BIT = [ + [0, 3], + [1, 4], + [2, 5], + [6, 7], +] + +_THRESHOLD = 180 + + +def _pixels_to_braille(pixels: list[list[int]]) -> str: + """Convert a 2-wide × 4-tall pixel block to a braille character.""" + bits = 0 + for row in range(4): + for col in range(2): + if pixels[row][col] < _THRESHOLD: + bits |= (1 << _DOT_BIT[row][col]) + return chr(0x2800 + bits) + + +def _image_to_braille(img: Image.Image, cols: int, rows: int) -> list[str]: + """Resize img to (cols*2) × (rows*4) and render as braille lines.""" + img = img.resize((cols * 2, rows * 4), Image.LANCZOS) + px = list(img.getdata()) + w = cols * 2 + + lines: list[str] = [] + for br in range(rows): + line = '' + for bc in range(cols): + block = [ + [px[(br * 4 + dr) * w + bc * 2 + dc] for dc in range(2)] + for dr in range(4) + ] + line += _pixels_to_braille(block) + lines.append(line) + return lines + + +# ── MapPanel ────────────────────────────────────────────────────────────────── + +class MapPanel: + """Async OSM tile fetcher and braille renderer. + + Call render() from the curses loop; it returns immediately with whatever + is cached. Tile fetches happen on background threads. + """ + + _ZOOM = 13 + # How many tiles to stitch in each direction around the centre tile. + _RADIUS = 1 # 3x3 grid + + def __init__(self) -> None: + self._lat: Optional[float] = None + self._lon: Optional[float] = None + self._lock = threading.Lock() + + # Last rendered state -- reuse if position hasn't changed much + self._last_render_lat: Optional[float] = None + self._last_render_lon: Optional[float] = None + self._last_render_size: tuple[int, int] = (0, 0) + self._cached_lines: list[str] = [] + self._cached_ts: float = 0.0 + + self._pending_fetches: set[tuple[int, int, int]] = set() + self._fetch_lock = threading.Lock() + + def update_position(self, lat: float, lon: float) -> None: + with self._lock: + self._lat = lat + self._lon = lon + + def _tiles_for_position(self, lat: float, lon: float) -> list[tuple[int, int, int]]: + cx, cy = _deg_to_tile(lat, lon, self._ZOOM) + r = self._RADIUS + return [ + (self._ZOOM, cx + dx, cy + dy) + for dy in range(-r, r + 1) + for dx in range(-r, r + 1) + ] + + def _ensure_tiles_fetched(self, tiles: list[tuple[int, int, int]]) -> None: + to_fetch = [] + with _tile_lock: + for key in tiles: + if key not in _tile_cache: + to_fetch.append(key) + + with self._fetch_lock: + for key in to_fetch: + if key not in self._pending_fetches: + self._pending_fetches.add(key) + z, x, y = key + t = threading.Thread( + target=self._fetch_and_clear, + args=(z, x, y), + daemon=True, + ) + t.start() + + def _fetch_and_clear(self, z: int, x: int, y: int) -> None: + _fetch_tile(z, x, y) + with self._fetch_lock: + self._pending_fetches.discard((z, x, y)) + + def _stitch_tiles( + self, + lat: float, + lon: float, + px_w: int, + px_h: int, + ) -> Image.Image: + cx, cy = _deg_to_tile(lat, lon, self._ZOOM) + r = self._RADIUS + grid_dim = 2 * r + 1 + canvas = Image.new('L', (grid_dim * _TILE_SIZE, grid_dim * _TILE_SIZE), color=200) + + for dy in range(-r, r + 1): + for dx in range(-r, r + 1): + tile = _fetch_tile(self._ZOOM, cx + dx, cy + dy) + if tile is not None: + canvas.paste(tile, ((dx + r) * _TILE_SIZE, (dy + r) * _TILE_SIZE)) + + # Pixel offset of (lat, lon) within the stitched canvas + tile_lat, tile_lon = _tile_origin_deg(cx - r, cy - r, self._ZOOM) + _, tile_lon_end = _tile_origin_deg(cx + r + 1, cy - r, self._ZOOM) + tile_lat_bot, _ = _tile_origin_deg(cx - r, cy + r + 1, self._ZOOM) + + lon_span = tile_lon_end - tile_lon + lat_span = tile_lat - tile_lat_bot + + if lon_span <= 0 or lat_span <= 0: + return canvas.crop((0, 0, px_w, px_h)) + + cx_px = int((lon - tile_lon) / lon_span * canvas.width) + cy_px = int((tile_lat - lat) / lat_span * canvas.height) + + # Crop centred on the vehicle position + left = max(0, cx_px - px_w // 2) + top = max(0, cy_px - px_h // 2) + right = left + px_w + bottom = top + px_h + + if right > canvas.width: + right = canvas.width + left = max(0, right - px_w) + if bottom > canvas.height: + bottom = canvas.height + top = max(0, bottom - px_h) + + return canvas.crop((left, top, right, bottom)), (cx_px - left, cy_px - top) + + def render(self, cols: int, rows: int) -> tuple[list[str], tuple[int, int] | None]: + """Return (braille_lines, marker_cell) where marker_cell is (col, row) + of the vehicle position in braille-cell coordinates, or None.""" + with self._lock: + lat = self._lat + lon = self._lon + + if lat is None or lon is None: + waiting = ['No position fix'] + [''] * (rows - 1) + return waiting, None + + tiles = self._tiles_for_position(lat, lon) + self._ensure_tiles_fetched(tiles) + + # Check cache validity: re-render if position moved >10m or size changed + if ( + self._last_render_size == (cols, rows) + and self._last_render_lat is not None + and abs(lat - self._last_render_lat) < 0.0001 + and abs(lon - self._last_render_lon) < 0.0001 + and self._cached_lines + and (time.monotonic() - self._cached_ts) < 10.0 + ): + with _tile_lock: + all_loaded = all(k in _tile_cache and _tile_cache[k] is not None for k in tiles) + if all_loaded: + return self._cached_lines, self._marker_cell + + px_w = cols * 2 + px_h = rows * 4 + + try: + result = self._stitch_tiles(lat, lon, px_w, px_h) + if isinstance(result, tuple): + canvas, (veh_px_x, veh_px_y) = result + else: + canvas = result + veh_px_x = px_w // 2 + veh_px_y = px_h // 2 + except Exception: + lines = ['Map error'] + [''] * (rows - 1) + return lines, None + + lines = _image_to_braille(canvas, cols, rows) + + # Braille-cell coordinates of the vehicle + marker_col = min(veh_px_x // 2, cols - 1) + marker_row = min(veh_px_y // 4, rows - 1) + + self._cached_lines = lines + self._marker_cell = (marker_col, marker_row) + self._last_render_lat = lat + self._last_render_lon = lon + self._last_render_size = (cols, rows) + self._cached_ts = time.monotonic() + + return lines, (marker_col, marker_row) diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/sources.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/sources.py new file mode 100644 index 00000000..cc8e0520 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/sources.py @@ -0,0 +1,251 @@ +from __future__ import annotations + +import time +import logging +from abc import ABC, abstractmethod +from typing import Optional + +import requests + +from .forecast import WeatherForecast, HourlyEntry + +log = logging.getLogger(__name__) + + +class WeatherSource(ABC): + name: str + + @abstractmethod + def fetch(self, lat: float, lon: float, timeout_s: float) -> Optional[WeatherForecast]: + ... + + +class NOAASource(WeatherSource): + name = 'noaa' + + _POINTS_URL = 'https://api.weather.gov/points/{lat},{lon}' + _HEADERS = {'User-Agent': 'weather_service_interface/1.0 (ros2-node)'} + + def fetch(self, lat: float, lon: float, timeout_s: float) -> Optional[WeatherForecast]: + try: + points_url = self._POINTS_URL.format(lat=round(lat, 4), lon=round(lon, 4)) + r = requests.get(points_url, headers=self._HEADERS, timeout=timeout_s) + if r.status_code == 404: + log.debug('NOAA: location outside coverage (%.4f, %.4f)', lat, lon) + return None + r.raise_for_status() + props = r.json()['properties'] + forecast_hourly_url = props['forecastHourly'] + + r2 = requests.get(forecast_hourly_url, headers=self._HEADERS, timeout=timeout_s) + r2.raise_for_status() + periods = r2.json()['properties']['periods'] + + hourly: list[HourlyEntry] = [] + for p in periods[:48]: + wind_spd_raw: str = p.get('windSpeed', '0 mph') + try: + wind_kmh = float(wind_spd_raw.split()[0]) * 1.60934 + except (ValueError, IndexError): + wind_kmh = 0.0 + + temp_c = (p['temperature'] - 32) * 5 / 9 if p.get('temperatureUnit') == 'F' else float(p['temperature']) + + hourly.append(HourlyEntry( + time_iso=p['startTime'], + temperature_c=round(temp_c, 1), + apparent_temperature_c=round(temp_c, 1), + precipitation_mm=0.0, + precipitation_probability_pct=float(p.get('probabilityOfPrecipitation', {}).get('value') or 0), + wind_speed_kmh=round(wind_kmh, 1), + wind_direction_deg=0.0, + wind_gusts_kmh=0.0, + cloud_cover_pct=0.0, + visibility_m=0.0, + weather_code=0, + weather_description=p.get('shortForecast', ''), + )) + + now = time.time() + return WeatherForecast( + source='noaa', + latitude=lat, + longitude=lon, + fetch_time_unix=now, + valid_until_unix=now + 3600 * 48, + timezone='UTC', + hourly=hourly, + ) + except requests.RequestException as e: + log.warning('NOAA fetch failed: %s', e) + return None + except Exception as e: + log.warning('NOAA parse error: %s', e) + return None + + +class DWDSource(WeatherSource): + """Deutscher Wetterdienst via Brightsky API. + + Default Brightsky units (no units param): temperature °C, wind km/h, + precipitation mm, precipitation_probability 0-100 integer. + """ + name = 'dwd' + + _URL = 'https://api.brightsky.dev/weather' + + def fetch(self, lat: float, lon: float, timeout_s: float) -> Optional[WeatherForecast]: + try: + from datetime import datetime, timezone, timedelta + now_utc = datetime.now(timezone.utc) + date_from = now_utc.strftime('%Y-%m-%dT%H:%M:%S') + date_to = (now_utc + timedelta(hours=48)).strftime('%Y-%m-%dT%H:%M:%S') + + params = { + 'lat': lat, + 'lon': lon, + 'date': date_from, + 'last_date': date_to, + } + r = requests.get(self._URL, params=params, timeout=timeout_s) + r.raise_for_status() + data = r.json() + + hourly: list[HourlyEntry] = [] + for entry in data.get('weather', []): + condition = entry.get('condition', '') + icon = entry.get('icon', '') + hourly.append(HourlyEntry( + time_iso=entry.get('timestamp', ''), + temperature_c=float(entry.get('temperature') or 0), + apparent_temperature_c=float(entry.get('temperature') or 0), + precipitation_mm=float(entry.get('precipitation') or 0), + precipitation_probability_pct=float(entry.get('precipitation_probability') or 0), + wind_speed_kmh=float(entry.get('wind_speed') or 0), + wind_direction_deg=float(entry.get('wind_direction') or 0), + wind_gusts_kmh=float(entry.get('wind_gust_speed') or 0), + cloud_cover_pct=float(entry.get('cloud_cover') or 0), + visibility_m=float(entry.get('visibility') or 0), + weather_code=0, + weather_description=condition or icon, + )) + + now_ts = time.time() + return WeatherForecast( + source='dwd', + latitude=lat, + longitude=lon, + fetch_time_unix=now_ts, + valid_until_unix=now_ts + 3600 * 48, + timezone=data.get('sources', [{}])[0].get('timezone', 'UTC'), + hourly=hourly, + ) + except requests.RequestException as e: + log.warning('DWD fetch failed: %s', e) + return None + except Exception as e: + log.warning('DWD parse error: %s', e) + return None + + +class OpenMeteoSource(WeatherSource): + """Open-Meteo: free, no API key, global coverage. Used as fallback.""" + name = 'open_meteo' + + _URL = 'https://api.open-meteo.com/v1/forecast' + + _HOURLY_VARS = [ + 'temperature_2m', + 'apparent_temperature', + 'precipitation', + 'precipitation_probability', + 'wind_speed_10m', + 'wind_direction_10m', + 'wind_gusts_10m', + 'cloud_cover', + 'visibility', + 'weather_code', + ] + + def fetch(self, lat: float, lon: float, timeout_s: float) -> Optional[WeatherForecast]: + try: + params = { + 'latitude': lat, + 'longitude': lon, + 'hourly': ','.join(self._HOURLY_VARS), + 'forecast_days': 2, + 'wind_speed_unit': 'kmh', + 'timezone': 'UTC', + } + r = requests.get(self._URL, params=params, timeout=timeout_s) + r.raise_for_status() + data = r.json() + + h = data.get('hourly', {}) + times = h.get('time', []) + n = len(times) + + def _col(key: str) -> list: + vals = h.get(key, [None] * n) + return [v if v is not None else 0 for v in vals] + + temps = _col('temperature_2m') + feels = _col('apparent_temperature') + precip = _col('precipitation') + precip_p = _col('precipitation_probability') + wspd = _col('wind_speed_10m') + wdir = _col('wind_direction_10m') + wgust = _col('wind_gusts_10m') + cloud = _col('cloud_cover') + vis = _col('visibility') + wcodes = _col('weather_code') + + hourly: list[HourlyEntry] = [] + for i in range(n): + code = int(wcodes[i]) + hourly.append(HourlyEntry( + time_iso=times[i], + temperature_c=float(temps[i]), + apparent_temperature_c=float(feels[i]), + precipitation_mm=float(precip[i]), + precipitation_probability_pct=float(precip_p[i]), + wind_speed_kmh=float(wspd[i]), + wind_direction_deg=float(wdir[i]), + wind_gusts_kmh=float(wgust[i]), + cloud_cover_pct=float(cloud[i]), + visibility_m=float(vis[i]), + weather_code=code, + weather_description=WeatherForecast.weather_code_description(code), + )) + + now = time.time() + return WeatherForecast( + source='open_meteo', + latitude=data.get('latitude', lat), + longitude=data.get('longitude', lon), + fetch_time_unix=now, + valid_until_unix=now + 3600 * 48, + timezone=data.get('timezone', 'UTC'), + hourly=hourly, + ) + except requests.RequestException as e: + log.warning('Open-Meteo fetch failed: %s', e) + return None + except Exception as e: + log.warning('Open-Meteo parse error: %s', e) + return None + + +def build_source_chain(sources_cfg: dict) -> list[WeatherSource]: + registry: dict[str, WeatherSource] = { + 'noaa': NOAASource(), + 'dwd': DWDSource(), + 'open_meteo': OpenMeteoSource(), + } + enabled = [ + (name, src) + for name, src in registry.items() + if sources_cfg.get(name, {}).get('enabled', True) + ] + enabled.sort(key=lambda x: sources_cfg.get(x[0], {}).get('priority', 99)) + return [src for _, src in enabled] diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/weather_service_node.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/weather_service_node.py new file mode 100644 index 00000000..aecd95e8 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/weather_service_node.py @@ -0,0 +1,221 @@ +from __future__ import annotations + +import json +import math +import os +import signal +import threading +import time +import yaml +from typing import Optional + +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from sensor_msgs.msg import NavSatFix +from std_msgs.msg import String +from ament_index_python.packages import get_package_share_directory + +from .forecast import WeatherForecast +from .forecast_logger import ForecastLogger +from .sources import build_source_chain + + +def _haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float: + R = 6_371_000.0 + phi1, phi2 = math.radians(lat1), math.radians(lat2) + dphi = math.radians(lat2 - lat1) + dlam = math.radians(lon2 - lon1) + a = math.sin(dphi / 2) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(dlam / 2) ** 2 + return 2 * R * math.asin(math.sqrt(a)) + + +class WeatherServiceNode(Node): + def __init__(self): + super().__init__('weather_service_node') + self.declare_parameter('config_path', '') + + config_path = self.get_parameter('config_path').get_parameter_value().string_value + if not config_path: + config_path = os.path.join( + get_package_share_directory('weather_service_interface'), + 'config', 'weather_service_config.yaml', + ) + + if not os.path.exists(config_path): + self.get_logger().error(f'Config not found: {config_path}') + raise RuntimeError(f'Config not found: {config_path}') + + with open(config_path, 'r') as f: + self._cfg = yaml.safe_load(f) + + fetch_cfg = self._cfg.get('fetch', {}) + log_cfg = self._cfg.get('logging', {}) + topics_cfg = self._cfg.get('topics', {}) + sources_cfg = self._cfg.get('sources', {}) + + self._displacement_threshold_m: float = fetch_cfg.get('displacement_threshold_m', 5000.0) + self._max_forecast_age_s: float = fetch_cfg.get('max_forecast_age_s', 3600.0) + self._check_interval_s: float = fetch_cfg.get('check_interval_s', 60.0) + self._publish_interval_s: float = fetch_cfg.get('publish_interval_s', 10.0) + + self._source_chain = build_source_chain(sources_cfg) + self._sources_cfg = sources_cfg + self._forecast_logger = ForecastLogger( + enabled=log_cfg.get('enabled', True), + max_files=int(log_cfg.get('max_files', 100)), + ) + + nav_sat_topic = topics_cfg.get('nav_sat_fix', '/ego_vehicle/vehicle_state_dynamic_nav_sat_fix') + forecast_topic = topics_cfg.get('weather_forecast', '/ego_vehicle/weather_forecast') + queue_depth = int(topics_cfg.get('queue_depth', 10)) + + self._current_forecast: Optional[WeatherForecast] = None + self._last_fetch_lat: Optional[float] = None + self._last_fetch_lon: Optional[float] = None + self._latest_lat: Optional[float] = None + self._latest_lon: Optional[float] = None + self._forecast_lock = threading.Lock() + self._position_lock = threading.Lock() + self._shutdown = threading.Event() + + self._sub = self.create_subscription( + NavSatFix, + nav_sat_topic, + self._nav_sat_callback, + queue_depth, + ) + self._pub = self.create_publisher(String, forecast_topic, queue_depth) + + self._fetch_timer = self.create_timer(self._check_interval_s, self._check_and_fetch) + self._publish_timer = self.create_timer(self._publish_interval_s, self._publish_forecast) + + if not self._source_chain: + self.get_logger().error('No weather sources enabled. Check config.') + else: + names = ', '.join(s.name for s in self._source_chain) + self.get_logger().info(f'Source priority chain: {names}') + + self.get_logger().info( + f'WeatherServiceNode ready | ' + f'displacement threshold: {self._displacement_threshold_m:.0f} m | ' + f'max forecast age: {self._max_forecast_age_s:.0f} s | ' + f'publish interval: {self._publish_interval_s:.1f} s' + ) + + # ── position subscription ───────────────────────────────────────────────── + + def _nav_sat_callback(self, msg: NavSatFix) -> None: + with self._position_lock: + self._latest_lat = msg.latitude + self._latest_lon = msg.longitude + + # ── fetch logic ─────────────────────────────────────────────────────────── + + def _needs_fetch(self, lat: float, lon: float) -> bool: + with self._forecast_lock: + if self._current_forecast is None: + return True + + if self._current_forecast.is_expired(self._max_forecast_age_s): + self.get_logger().info('Forecast expired, triggering fetch.') + return True + + if self._last_fetch_lat is None or self._last_fetch_lon is None: + return True + + dist = _haversine_m(self._last_fetch_lat, self._last_fetch_lon, lat, lon) + if dist >= self._displacement_threshold_m: + self.get_logger().info( + f'Vehicle displaced {dist:.0f} m (threshold: {self._displacement_threshold_m:.0f} m), ' + f'triggering fetch.' + ) + return True + + return False + + def _do_fetch(self, lat: float, lon: float) -> None: + for source in self._source_chain: + timeout = self._sources_cfg.get(source.name, {}).get('timeout_s', 10.0) + self.get_logger().info(f'Attempting fetch from {source.name} ({lat:.5f}, {lon:.5f})') + try: + forecast = source.fetch(lat, lon, timeout) + except Exception as e: + self.get_logger().warning(f'{source.name} raised exception: {e}') + forecast = None + + if forecast is not None and forecast.hourly: + with self._forecast_lock: + self._current_forecast = forecast + self._last_fetch_lat = lat + self._last_fetch_lon = lon + self._forecast_logger.log(forecast) + self.get_logger().info( + f'Forecast updated from {source.name}: ' + f'{len(forecast.hourly)} hourly entries.' + ) + return + + self.get_logger().warning(f'{source.name} returned no usable data, trying next source.') + + self.get_logger().error('All weather sources failed. Retaining previous forecast if available.') + + def _check_and_fetch(self) -> None: + with self._position_lock: + lat = self._latest_lat + lon = self._latest_lon + + if lat is None or lon is None: + self.get_logger().debug('No vehicle position yet, skipping fetch check.') + return + + if self._needs_fetch(lat, lon): + fetch_thread = threading.Thread( + target=self._do_fetch, args=(lat, lon), daemon=True + ) + fetch_thread.start() + + # ── publish ─────────────────────────────────────────────────────────────── + + def _publish_forecast(self) -> None: + with self._forecast_lock: + forecast = self._current_forecast + + if forecast is None: + return + + msg = String() + msg.data = json.dumps(forecast.to_dict()) + self._pub.publish(msg) + + def shutdown(self) -> None: + self._shutdown.set() + + +def main(args=None): + rclpy.init(args=args) + try: + node = WeatherServiceNode() + except RuntimeError: + rclpy.shutdown() + return + + executor = MultiThreadedExecutor() + executor.add_node(node) + + shutdown_event = threading.Event() + + def _signal_handler(sig, frame): + shutdown_event.set() + + signal.signal(signal.SIGINT, _signal_handler) + signal.signal(signal.SIGTERM, _signal_handler) + + try: + while not shutdown_event.is_set(): + executor.spin_once(timeout_sec=0.1) + finally: + node.shutdown() + executor.shutdown() + node.destroy_node() + rclpy.shutdown() diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/weather_visualizer_node.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/weather_visualizer_node.py new file mode 100644 index 00000000..dd6b628d --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_interface/weather_visualizer_node.py @@ -0,0 +1,363 @@ +from __future__ import annotations + +import curses +import json +import os +import signal +import threading +import time +from datetime import datetime, timezone, timedelta +from typing import Optional + +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from std_msgs.msg import String + +from .forecast import WeatherForecast, HourlyEntry + + +def _wind_compass(deg: float) -> str: + dirs = ['N', 'NE', 'E', 'SE', 'S', 'SW', 'W', 'NW'] + return dirs[int((deg + 22.5) / 45.0) % 8] + + +def _wind_arrow(deg: float) -> str: + arrows = ['↑', '↗', '→', '↘', '↓', '↙', '←', '↖'] + return arrows[int((deg + 22.5) / 45.0) % 8] + + +_SPARKS = ' ▁▂▃▄▅▆▇█' + + +def _sparkline(values: list[float], width: int) -> str: + if not values: + return ' ' * width + samples = values[:width] + lo, hi = min(samples), max(samples) + span = hi - lo or 1 + return ''.join(_SPARKS[min(int((v - lo) / span * 8), 8)] for v in samples) + + +def _upcoming_hours(hourly: list[HourlyEntry], hours: int = 12) -> list[HourlyEntry]: + """Return entries from the next upcoming hour onward, up to `hours` entries.""" + now_utc = datetime.now(timezone.utc) + result: list[HourlyEntry] = [] + for entry in hourly: + try: + ts = entry.time_iso + # Brightsky returns RFC3339 with offset; Open-Meteo returns naive UTC + if ts.endswith('Z'): + ts = ts[:-1] + '+00:00' + dt = datetime.fromisoformat(ts) + if dt.tzinfo is None: + dt = dt.replace(tzinfo=timezone.utc) + if dt >= now_utc - timedelta(minutes=30): + result.append(entry) + if len(result) >= hours: + break + except ValueError: + continue + return result + + +class WeatherVisualizerNode(Node): + def __init__(self): + super().__init__('weather_visualizer_node') + self.declare_parameter('topic', '/ego_vehicle/weather_forecast') + self.declare_parameter('queue_depth', 10) + + topic = self.get_parameter('topic').get_parameter_value().string_value + queue_depth = self.get_parameter('queue_depth').get_parameter_value().integer_value + + self._forecast: Optional[WeatherForecast] = None + self._lock = threading.Lock() + self._shutdown = threading.Event() + self._last_msg_t: Optional[float] = None + + self._sub = self.create_subscription( + String, + topic, + self._forecast_callback, + queue_depth, + ) + self.get_logger().info(f'WeatherVisualizerNode watching {topic}') + + def _forecast_callback(self, msg: String) -> None: + try: + data = json.loads(msg.data) + hourly = [HourlyEntry(**e) for e in data.pop('hourly', [])] + forecast = WeatherForecast(**data, hourly=hourly) + with self._lock: + self._forecast = forecast + self._last_msg_t = time.monotonic() + except Exception as e: + self.get_logger().warning(f'Failed to parse forecast message: {e}') + + def get_state(self) -> tuple[Optional[WeatherForecast], Optional[float]]: + with self._lock: + return self._forecast, self._last_msg_t + + def is_shutdown(self) -> bool: + return self._shutdown.is_set() + + def shutdown(self) -> None: + self._shutdown.set() + + +# ── curses rendering (runs on main thread) ──────────────────────────────────── + +def _safe_addstr(win, y: int, x: int, text: str, attr: int = 0) -> None: + try: + win.addstr(y, x, text, attr) + except curses.error: + pass + + +def _draw_header(stdscr, width: int, forecast: Optional[WeatherForecast], stale: bool) -> None: + title = ' WEATHER FORECAST DASHBOARD ' + pad = max(0, (width - len(title)) // 2) + _safe_addstr(stdscr, 0, pad, title, curses.color_pair(1) | curses.A_BOLD | curses.A_REVERSE) + + if forecast: + age_s = int(time.time() - forecast.fetch_time_unix) + age_str = f'{age_s // 60}m{age_s % 60:02d}s ago' + src_line = ( + f'Source: {forecast.source.upper()} | ' + f'({forecast.latitude:.4f}, {forecast.longitude:.4f}) | ' + f'Fetched: {age_str}' + ) + else: + src_line = 'Waiting for forecast data...' + + dot = '●' if not stale else '○' + dot_color = curses.color_pair(4) if not stale else curses.color_pair(5) + _safe_addstr(stdscr, 1, 1, dot, dot_color) + _safe_addstr(stdscr, 1, 3, src_line[:width - 4], curses.color_pair(7)) + + +def _draw_waiting(stdscr, height: int, width: int) -> None: + msg = '[ Waiting for /ego_vehicle/weather_forecast ]' + _safe_addstr(stdscr, height // 2, max(0, (width - len(msg)) // 2), + msg, curses.color_pair(3) | curses.A_BOLD) + + +def _draw_current(stdscr, entry: HourlyEntry, width: int) -> None: + temp_color = curses.color_pair(2) if entry.temperature_c > 20 else ( + curses.color_pair(3) if entry.temperature_c < 5 else curses.color_pair(6) + ) + time_label = entry.time_iso[11:16] if len(entry.time_iso) >= 16 else '' + _safe_addstr(stdscr, 3, 2, f'NEXT HOUR {time_label}', curses.color_pair(1) | curses.A_BOLD) + _safe_addstr(stdscr, 3, 22, + f'{entry.temperature_c:+.1f}°C feels {entry.apparent_temperature_c:+.1f}°C', + temp_color | curses.A_BOLD, + ) + _safe_addstr(stdscr, 4, 2, + f'Wind: {_wind_arrow(entry.wind_direction_deg)} {_wind_compass(entry.wind_direction_deg)} ' + f'{entry.wind_speed_kmh:.0f} km/h gusts {entry.wind_gusts_kmh:.0f} km/h', + curses.color_pair(6), + ) + precip_color = curses.color_pair(3) if entry.precipitation_mm > 0.5 else curses.color_pair(6) + _safe_addstr(stdscr, 5, 2, + f'Precip: {entry.precipitation_mm:.1f} mm ({entry.precipitation_probability_pct:.0f}% chance)', + precip_color, + ) + _safe_addstr(stdscr, 6, 2, + f'Cloud: {entry.cloud_cover_pct:.0f}% Visibility: {entry.visibility_m / 1000:.1f} km', + curses.color_pair(6), + ) + _safe_addstr(stdscr, 7, 2, + f'Conditions: {entry.weather_description[:width - 16]}', + curses.color_pair(6) | curses.A_ITALIC, + ) + + +def _draw_hourly_table(stdscr, entries: list[HourlyEntry], height: int, width: int) -> int: + row = 9 + if row >= height - 4: + return row + + header = ( + f"{'Time':>5} {'°C':>5} {'Fl':>5} {'Prcp':>5} " + f"{'Prob':>4} {'Wspd':>5} {'Gust':>5} {'Dir':>3} {'Cld':>3} Conditions" + ) + _safe_addstr(stdscr, row, 2, header[:width - 3], curses.color_pair(1) | curses.A_UNDERLINE) + row += 1 + + for entry in entries: + if row >= height - 4: + break + time_part = entry.time_iso[11:16] if len(entry.time_iso) >= 16 else entry.time_iso[:5] + temp_color = curses.color_pair(2) if entry.temperature_c > 20 else ( + curses.color_pair(3) if entry.temperature_c < 5 else curses.color_pair(6) + ) + line = ( + f"{time_part:>5} " + f"{entry.temperature_c:>+5.1f} " + f"{entry.apparent_temperature_c:>+5.1f} " + f"{entry.precipitation_mm:>5.1f} " + f"{entry.precipitation_probability_pct:>3.0f}% " + f"{entry.wind_speed_kmh:>5.0f} " + f"{entry.wind_gusts_kmh:>5.0f} " + f"{_wind_compass(entry.wind_direction_deg):>3} " + f"{entry.cloud_cover_pct:>3.0f}% " + f"{entry.weather_description[:20]}" + ) + _safe_addstr(stdscr, row, 2, line[:width - 3], temp_color) + row += 1 + + return row + + +def _draw_sparklines(stdscr, entries: list[HourlyEntry], height: int, width: int) -> None: + spark_row = height - 4 + if spark_row < 10: + return + spark_width = min(len(entries), width - 20) + temps = [h.temperature_c for h in entries] + precip = [h.precipitation_mm for h in entries] + _safe_addstr(stdscr, spark_row, 2, 'Temp 12h: ', curses.color_pair(2)) + _safe_addstr(stdscr, spark_row, 13, _sparkline(temps, spark_width), curses.color_pair(2) | curses.A_BOLD) + _safe_addstr(stdscr, spark_row + 1, 2, 'Precip 12h:', curses.color_pair(3)) + _safe_addstr(stdscr, spark_row + 1, 13, _sparkline(precip, spark_width), curses.color_pair(3) | curses.A_BOLD) + + +def _draw_footer(stdscr, height: int, width: int) -> None: + footer = ' [q] Quit ' + _safe_addstr(stdscr, height - 1, 0, footer.ljust(width - 1)[:width - 1], curses.A_REVERSE) + + +def _curses_loop(stdscr, node: WeatherVisualizerNode) -> None: + curses.curs_set(0) + stdscr.nodelay(True) + stdscr.timeout(500) + + curses.start_color() + curses.use_default_colors() + curses.init_pair(1, curses.COLOR_CYAN, -1) + curses.init_pair(2, curses.COLOR_YELLOW, -1) + curses.init_pair(3, curses.COLOR_BLUE, -1) + curses.init_pair(4, curses.COLOR_GREEN, -1) + curses.init_pair(5, curses.COLOR_RED, -1) + curses.init_pair(6, curses.COLOR_WHITE, -1) + curses.init_pair(7, curses.COLOR_MAGENTA, -1) + + while not node.is_shutdown(): + key = stdscr.getch() + if key in (ord('q'), ord('Q'), 27): + node.shutdown() + break + + forecast, last_t = node.get_state() + stale = (last_t is None) or ((time.monotonic() - last_t) > 120) + + height, width = stdscr.getmaxyx() + stdscr.erase() + + _draw_header(stdscr, width, forecast, stale) + + if forecast is None or not forecast.hourly: + _draw_waiting(stdscr, height, width) + else: + upcoming = _upcoming_hours(forecast.hourly, hours=12) + if not upcoming: + _draw_waiting(stdscr, height, width) + else: + _draw_current(stdscr, upcoming[0], width) + _draw_hourly_table(stdscr, upcoming, height, width) + _draw_sparklines(stdscr, upcoming, height, width) + + _draw_footer(stdscr, height, width) + try: + stdscr.refresh() + except curses.error: + pass + + +def _reexec_in_xterm_if_needed() -> None: + """Re-exec under xterm when visualizer.use_xterm is set in the config. + + Guarded by WEATHER_VIZ_IN_XTERM to prevent re-exec loops. Also used by + the launch file prefix approach, which sets the flag directly. + """ + import shutil + import sys + + if os.environ.get('WEATHER_VIZ_IN_XTERM'): + return + if not shutil.which('xterm'): + return + + config_path = os.environ.get('WEATHER_CONFIG_PATH') + if not config_path: + # Best-effort: locate the installed package config + try: + from ament_index_python.packages import get_package_share_directory + config_path = os.path.join( + get_package_share_directory('weather_service_interface'), + 'config', 'weather_service_config.yaml', + ) + except Exception: + return + + try: + import yaml + with open(config_path) as f: + cfg = yaml.safe_load(f) + use_xterm = bool(cfg.get('visualizer', {}).get('use_xterm', False)) + except Exception: + return + + if not use_xterm: + return + + env = os.environ.copy() + env['WEATHER_VIZ_IN_XTERM'] = '1' + os.execvpe('xterm', ['xterm', '-e'] + sys.argv, env) + + +def main(args=None): + _reexec_in_xterm_if_needed() + rclpy.init(args=args) + try: + node = WeatherVisualizerNode() + except Exception: + rclpy.shutdown() + return + + executor = MultiThreadedExecutor() + executor.add_node(node) + + # Spin the executor on a background thread so the main thread can own the TTY for curses + spin_thread = threading.Thread( + target=lambda: _spin_until_shutdown(executor, node), + daemon=True, + ) + + shutdown_event = threading.Event() + + def _signal_handler(sig, frame): + shutdown_event.set() + node.shutdown() + + signal.signal(signal.SIGINT, _signal_handler) + signal.signal(signal.SIGTERM, _signal_handler) + + spin_thread.start() + + try: + curses.wrapper(_curses_loop, node) + except Exception as e: + node.get_logger().error(f'Curses error: {e}') + finally: + node.shutdown() + shutdown_event.set() + spin_thread.join(timeout=2.0) + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + + +def _spin_until_shutdown(executor: MultiThreadedExecutor, node: WeatherVisualizerNode) -> None: + while not node.is_shutdown(): + executor.spin_once(timeout_sec=0.1) diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_node.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_node.py new file mode 100644 index 00000000..aecd95e8 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_service_node.py @@ -0,0 +1,221 @@ +from __future__ import annotations + +import json +import math +import os +import signal +import threading +import time +import yaml +from typing import Optional + +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from sensor_msgs.msg import NavSatFix +from std_msgs.msg import String +from ament_index_python.packages import get_package_share_directory + +from .forecast import WeatherForecast +from .forecast_logger import ForecastLogger +from .sources import build_source_chain + + +def _haversine_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float: + R = 6_371_000.0 + phi1, phi2 = math.radians(lat1), math.radians(lat2) + dphi = math.radians(lat2 - lat1) + dlam = math.radians(lon2 - lon1) + a = math.sin(dphi / 2) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(dlam / 2) ** 2 + return 2 * R * math.asin(math.sqrt(a)) + + +class WeatherServiceNode(Node): + def __init__(self): + super().__init__('weather_service_node') + self.declare_parameter('config_path', '') + + config_path = self.get_parameter('config_path').get_parameter_value().string_value + if not config_path: + config_path = os.path.join( + get_package_share_directory('weather_service_interface'), + 'config', 'weather_service_config.yaml', + ) + + if not os.path.exists(config_path): + self.get_logger().error(f'Config not found: {config_path}') + raise RuntimeError(f'Config not found: {config_path}') + + with open(config_path, 'r') as f: + self._cfg = yaml.safe_load(f) + + fetch_cfg = self._cfg.get('fetch', {}) + log_cfg = self._cfg.get('logging', {}) + topics_cfg = self._cfg.get('topics', {}) + sources_cfg = self._cfg.get('sources', {}) + + self._displacement_threshold_m: float = fetch_cfg.get('displacement_threshold_m', 5000.0) + self._max_forecast_age_s: float = fetch_cfg.get('max_forecast_age_s', 3600.0) + self._check_interval_s: float = fetch_cfg.get('check_interval_s', 60.0) + self._publish_interval_s: float = fetch_cfg.get('publish_interval_s', 10.0) + + self._source_chain = build_source_chain(sources_cfg) + self._sources_cfg = sources_cfg + self._forecast_logger = ForecastLogger( + enabled=log_cfg.get('enabled', True), + max_files=int(log_cfg.get('max_files', 100)), + ) + + nav_sat_topic = topics_cfg.get('nav_sat_fix', '/ego_vehicle/vehicle_state_dynamic_nav_sat_fix') + forecast_topic = topics_cfg.get('weather_forecast', '/ego_vehicle/weather_forecast') + queue_depth = int(topics_cfg.get('queue_depth', 10)) + + self._current_forecast: Optional[WeatherForecast] = None + self._last_fetch_lat: Optional[float] = None + self._last_fetch_lon: Optional[float] = None + self._latest_lat: Optional[float] = None + self._latest_lon: Optional[float] = None + self._forecast_lock = threading.Lock() + self._position_lock = threading.Lock() + self._shutdown = threading.Event() + + self._sub = self.create_subscription( + NavSatFix, + nav_sat_topic, + self._nav_sat_callback, + queue_depth, + ) + self._pub = self.create_publisher(String, forecast_topic, queue_depth) + + self._fetch_timer = self.create_timer(self._check_interval_s, self._check_and_fetch) + self._publish_timer = self.create_timer(self._publish_interval_s, self._publish_forecast) + + if not self._source_chain: + self.get_logger().error('No weather sources enabled. Check config.') + else: + names = ', '.join(s.name for s in self._source_chain) + self.get_logger().info(f'Source priority chain: {names}') + + self.get_logger().info( + f'WeatherServiceNode ready | ' + f'displacement threshold: {self._displacement_threshold_m:.0f} m | ' + f'max forecast age: {self._max_forecast_age_s:.0f} s | ' + f'publish interval: {self._publish_interval_s:.1f} s' + ) + + # ── position subscription ───────────────────────────────────────────────── + + def _nav_sat_callback(self, msg: NavSatFix) -> None: + with self._position_lock: + self._latest_lat = msg.latitude + self._latest_lon = msg.longitude + + # ── fetch logic ─────────────────────────────────────────────────────────── + + def _needs_fetch(self, lat: float, lon: float) -> bool: + with self._forecast_lock: + if self._current_forecast is None: + return True + + if self._current_forecast.is_expired(self._max_forecast_age_s): + self.get_logger().info('Forecast expired, triggering fetch.') + return True + + if self._last_fetch_lat is None or self._last_fetch_lon is None: + return True + + dist = _haversine_m(self._last_fetch_lat, self._last_fetch_lon, lat, lon) + if dist >= self._displacement_threshold_m: + self.get_logger().info( + f'Vehicle displaced {dist:.0f} m (threshold: {self._displacement_threshold_m:.0f} m), ' + f'triggering fetch.' + ) + return True + + return False + + def _do_fetch(self, lat: float, lon: float) -> None: + for source in self._source_chain: + timeout = self._sources_cfg.get(source.name, {}).get('timeout_s', 10.0) + self.get_logger().info(f'Attempting fetch from {source.name} ({lat:.5f}, {lon:.5f})') + try: + forecast = source.fetch(lat, lon, timeout) + except Exception as e: + self.get_logger().warning(f'{source.name} raised exception: {e}') + forecast = None + + if forecast is not None and forecast.hourly: + with self._forecast_lock: + self._current_forecast = forecast + self._last_fetch_lat = lat + self._last_fetch_lon = lon + self._forecast_logger.log(forecast) + self.get_logger().info( + f'Forecast updated from {source.name}: ' + f'{len(forecast.hourly)} hourly entries.' + ) + return + + self.get_logger().warning(f'{source.name} returned no usable data, trying next source.') + + self.get_logger().error('All weather sources failed. Retaining previous forecast if available.') + + def _check_and_fetch(self) -> None: + with self._position_lock: + lat = self._latest_lat + lon = self._latest_lon + + if lat is None or lon is None: + self.get_logger().debug('No vehicle position yet, skipping fetch check.') + return + + if self._needs_fetch(lat, lon): + fetch_thread = threading.Thread( + target=self._do_fetch, args=(lat, lon), daemon=True + ) + fetch_thread.start() + + # ── publish ─────────────────────────────────────────────────────────────── + + def _publish_forecast(self) -> None: + with self._forecast_lock: + forecast = self._current_forecast + + if forecast is None: + return + + msg = String() + msg.data = json.dumps(forecast.to_dict()) + self._pub.publish(msg) + + def shutdown(self) -> None: + self._shutdown.set() + + +def main(args=None): + rclpy.init(args=args) + try: + node = WeatherServiceNode() + except RuntimeError: + rclpy.shutdown() + return + + executor = MultiThreadedExecutor() + executor.add_node(node) + + shutdown_event = threading.Event() + + def _signal_handler(sig, frame): + shutdown_event.set() + + signal.signal(signal.SIGINT, _signal_handler) + signal.signal(signal.SIGTERM, _signal_handler) + + try: + while not shutdown_event.is_set(): + executor.spin_once(timeout_sec=0.1) + finally: + node.shutdown() + executor.shutdown() + node.destroy_node() + rclpy.shutdown() diff --git a/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_visualizer_node.py b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_visualizer_node.py new file mode 100644 index 00000000..764e1dc0 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/weather_service_interface/weather_visualizer_node.py @@ -0,0 +1,319 @@ +from __future__ import annotations + +import curses +import json +import signal +import threading +import time +from datetime import datetime, timezone, timedelta +from typing import Optional + +import rclpy +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from std_msgs.msg import String + +from .forecast import WeatherForecast, HourlyEntry + + +def _wind_compass(deg: float) -> str: + dirs = ['N', 'NE', 'E', 'SE', 'S', 'SW', 'W', 'NW'] + return dirs[int((deg + 22.5) / 45.0) % 8] + + +def _wind_arrow(deg: float) -> str: + arrows = ['↑', '↗', '→', '↘', '↓', '↙', '←', '↖'] + return arrows[int((deg + 22.5) / 45.0) % 8] + + +_SPARKS = ' ▁▂▃▄▅▆▇█' + + +def _sparkline(values: list[float], width: int) -> str: + if not values: + return ' ' * width + samples = values[:width] + lo, hi = min(samples), max(samples) + span = hi - lo or 1 + return ''.join(_SPARKS[min(int((v - lo) / span * 8), 8)] for v in samples) + + +def _upcoming_hours(hourly: list[HourlyEntry], hours: int = 12) -> list[HourlyEntry]: + """Return entries from the next upcoming hour onward, up to `hours` entries.""" + now_utc = datetime.now(timezone.utc) + result: list[HourlyEntry] = [] + for entry in hourly: + try: + ts = entry.time_iso + # Brightsky returns RFC3339 with offset; Open-Meteo returns naive UTC + if ts.endswith('Z'): + ts = ts[:-1] + '+00:00' + dt = datetime.fromisoformat(ts) + if dt.tzinfo is None: + dt = dt.replace(tzinfo=timezone.utc) + if dt >= now_utc - timedelta(minutes=30): + result.append(entry) + if len(result) >= hours: + break + except ValueError: + continue + return result + + +class WeatherVisualizerNode(Node): + def __init__(self): + super().__init__('weather_visualizer_node') + self.declare_parameter('topic', '/ego_vehicle/weather_forecast') + self.declare_parameter('queue_depth', 10) + + topic = self.get_parameter('topic').get_parameter_value().string_value + queue_depth = self.get_parameter('queue_depth').get_parameter_value().integer_value + + self._forecast: Optional[WeatherForecast] = None + self._lock = threading.Lock() + self._shutdown = threading.Event() + self._last_msg_t: Optional[float] = None + + self._sub = self.create_subscription( + String, + topic, + self._forecast_callback, + queue_depth, + ) + self.get_logger().info(f'WeatherVisualizerNode watching {topic}') + + def _forecast_callback(self, msg: String) -> None: + try: + data = json.loads(msg.data) + hourly = [HourlyEntry(**e) for e in data.pop('hourly', [])] + forecast = WeatherForecast(**data, hourly=hourly) + with self._lock: + self._forecast = forecast + self._last_msg_t = time.monotonic() + except Exception as e: + self.get_logger().warning(f'Failed to parse forecast message: {e}') + + def get_state(self) -> tuple[Optional[WeatherForecast], Optional[float]]: + with self._lock: + return self._forecast, self._last_msg_t + + def is_shutdown(self) -> bool: + return self._shutdown.is_set() + + def shutdown(self) -> None: + self._shutdown.set() + + +# ── curses rendering (runs on main thread) ──────────────────────────────────── + +def _safe_addstr(win, y: int, x: int, text: str, attr: int = 0) -> None: + try: + win.addstr(y, x, text, attr) + except curses.error: + pass + + +def _draw_header(stdscr, width: int, forecast: Optional[WeatherForecast], stale: bool) -> None: + title = ' WEATHER FORECAST DASHBOARD ' + pad = max(0, (width - len(title)) // 2) + _safe_addstr(stdscr, 0, pad, title, curses.color_pair(1) | curses.A_BOLD | curses.A_REVERSE) + + if forecast: + age_s = int(time.time() - forecast.fetch_time_unix) + age_str = f'{age_s // 60}m{age_s % 60:02d}s ago' + src_line = ( + f'Source: {forecast.source.upper()} | ' + f'({forecast.latitude:.4f}, {forecast.longitude:.4f}) | ' + f'Fetched: {age_str}' + ) + else: + src_line = 'Waiting for forecast data...' + + dot = '●' if not stale else '○' + dot_color = curses.color_pair(4) if not stale else curses.color_pair(5) + _safe_addstr(stdscr, 1, 1, dot, dot_color) + _safe_addstr(stdscr, 1, 3, src_line[:width - 4], curses.color_pair(7)) + + +def _draw_waiting(stdscr, height: int, width: int) -> None: + msg = '[ Waiting for /ego_vehicle/weather_forecast ]' + _safe_addstr(stdscr, height // 2, max(0, (width - len(msg)) // 2), + msg, curses.color_pair(3) | curses.A_BOLD) + + +def _draw_current(stdscr, entry: HourlyEntry, width: int) -> None: + temp_color = curses.color_pair(2) if entry.temperature_c > 20 else ( + curses.color_pair(3) if entry.temperature_c < 5 else curses.color_pair(6) + ) + time_label = entry.time_iso[11:16] if len(entry.time_iso) >= 16 else '' + _safe_addstr(stdscr, 3, 2, f'NEXT HOUR {time_label}', curses.color_pair(1) | curses.A_BOLD) + _safe_addstr(stdscr, 3, 22, + f'{entry.temperature_c:+.1f}°C feels {entry.apparent_temperature_c:+.1f}°C', + temp_color | curses.A_BOLD, + ) + _safe_addstr(stdscr, 4, 2, + f'Wind: {_wind_arrow(entry.wind_direction_deg)} {_wind_compass(entry.wind_direction_deg)} ' + f'{entry.wind_speed_kmh:.0f} km/h gusts {entry.wind_gusts_kmh:.0f} km/h', + curses.color_pair(6), + ) + precip_color = curses.color_pair(3) if entry.precipitation_mm > 0.5 else curses.color_pair(6) + _safe_addstr(stdscr, 5, 2, + f'Precip: {entry.precipitation_mm:.1f} mm ({entry.precipitation_probability_pct:.0f}% chance)', + precip_color, + ) + _safe_addstr(stdscr, 6, 2, + f'Cloud: {entry.cloud_cover_pct:.0f}% Visibility: {entry.visibility_m / 1000:.1f} km', + curses.color_pair(6), + ) + _safe_addstr(stdscr, 7, 2, + f'Conditions: {entry.weather_description[:width - 16]}', + curses.color_pair(6) | curses.A_ITALIC, + ) + + +def _draw_hourly_table(stdscr, entries: list[HourlyEntry], height: int, width: int) -> int: + row = 9 + if row >= height - 4: + return row + + header = ( + f"{'Time':>5} {'°C':>5} {'Fl':>5} {'Prcp':>5} " + f"{'Prob':>4} {'Wspd':>5} {'Gust':>5} {'Dir':>3} {'Cld':>3} Conditions" + ) + _safe_addstr(stdscr, row, 2, header[:width - 3], curses.color_pair(1) | curses.A_UNDERLINE) + row += 1 + + for entry in entries: + if row >= height - 4: + break + time_part = entry.time_iso[11:16] if len(entry.time_iso) >= 16 else entry.time_iso[:5] + temp_color = curses.color_pair(2) if entry.temperature_c > 20 else ( + curses.color_pair(3) if entry.temperature_c < 5 else curses.color_pair(6) + ) + line = ( + f"{time_part:>5} " + f"{entry.temperature_c:>+5.1f} " + f"{entry.apparent_temperature_c:>+5.1f} " + f"{entry.precipitation_mm:>5.1f} " + f"{entry.precipitation_probability_pct:>3.0f}% " + f"{entry.wind_speed_kmh:>5.0f} " + f"{entry.wind_gusts_kmh:>5.0f} " + f"{_wind_compass(entry.wind_direction_deg):>3} " + f"{entry.cloud_cover_pct:>3.0f}% " + f"{entry.weather_description[:20]}" + ) + _safe_addstr(stdscr, row, 2, line[:width - 3], temp_color) + row += 1 + + return row + + +def _draw_sparklines(stdscr, entries: list[HourlyEntry], height: int, width: int) -> None: + spark_row = height - 4 + if spark_row < 10: + return + spark_width = min(len(entries), width - 20) + temps = [h.temperature_c for h in entries] + precip = [h.precipitation_mm for h in entries] + _safe_addstr(stdscr, spark_row, 2, 'Temp 12h: ', curses.color_pair(2)) + _safe_addstr(stdscr, spark_row, 13, _sparkline(temps, spark_width), curses.color_pair(2) | curses.A_BOLD) + _safe_addstr(stdscr, spark_row + 1, 2, 'Precip 12h:', curses.color_pair(3)) + _safe_addstr(stdscr, spark_row + 1, 13, _sparkline(precip, spark_width), curses.color_pair(3) | curses.A_BOLD) + + +def _draw_footer(stdscr, height: int, width: int) -> None: + footer = ' [q] Quit ' + _safe_addstr(stdscr, height - 1, 0, footer.ljust(width - 1)[:width - 1], curses.A_REVERSE) + + +def _curses_loop(stdscr, node: WeatherVisualizerNode) -> None: + curses.curs_set(0) + stdscr.nodelay(True) + stdscr.timeout(500) + + curses.start_color() + curses.use_default_colors() + curses.init_pair(1, curses.COLOR_CYAN, -1) + curses.init_pair(2, curses.COLOR_YELLOW, -1) + curses.init_pair(3, curses.COLOR_BLUE, -1) + curses.init_pair(4, curses.COLOR_GREEN, -1) + curses.init_pair(5, curses.COLOR_RED, -1) + curses.init_pair(6, curses.COLOR_WHITE, -1) + curses.init_pair(7, curses.COLOR_MAGENTA, -1) + + while not node.is_shutdown(): + key = stdscr.getch() + if key in (ord('q'), ord('Q'), 27): + node.shutdown() + break + + forecast, last_t = node.get_state() + stale = (last_t is None) or ((time.monotonic() - last_t) > 120) + + height, width = stdscr.getmaxyx() + stdscr.erase() + + _draw_header(stdscr, width, forecast, stale) + + if forecast is None or not forecast.hourly: + _draw_waiting(stdscr, height, width) + else: + upcoming = _upcoming_hours(forecast.hourly, hours=12) + if not upcoming: + _draw_waiting(stdscr, height, width) + else: + _draw_current(stdscr, upcoming[0], width) + _draw_hourly_table(stdscr, upcoming, height, width) + _draw_sparklines(stdscr, upcoming, height, width) + + _draw_footer(stdscr, height, width) + try: + stdscr.refresh() + except curses.error: + pass + + +def main(args=None): + rclpy.init(args=args) + try: + node = WeatherVisualizerNode() + except Exception: + rclpy.shutdown() + return + + executor = MultiThreadedExecutor() + executor.add_node(node) + + # Spin the executor on a background thread so the main thread can own the TTY for curses + spin_thread = threading.Thread( + target=lambda: _spin_until_shutdown(executor, node), + daemon=True, + ) + + shutdown_event = threading.Event() + + def _signal_handler(sig, frame): + shutdown_event.set() + node.shutdown() + + signal.signal(signal.SIGINT, _signal_handler) + signal.signal(signal.SIGTERM, _signal_handler) + + spin_thread.start() + + try: + curses.wrapper(_curses_loop, node) + except Exception as e: + node.get_logger().error(f'Curses error: {e}') + finally: + node.shutdown() + shutdown_event.set() + spin_thread.join(timeout=2.0) + executor.shutdown() + node.destroy_node() + rclpy.shutdown() + + +def _spin_until_shutdown(executor: MultiThreadedExecutor, node: WeatherVisualizerNode) -> None: + while not node.is_shutdown(): + executor.spin_once(timeout_sec=0.1) From 6e28b3d828020557b00b57d95700a123f526add1 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Tue, 9 Jun 2026 10:47:46 +0200 Subject: [PATCH 02/18] updated mqtt, added connection parameters to bridge config file Signed-off-by: Andrew Koerner --- .../mqtt_message_bridge/.gitignore | 1 + .../config/bridge_config.yaml | 36 ++++++ .../launch/bridge.launch.py | 10 +- .../mqtt_message_bridge/bridge_node.py | 103 ++++++++++++++++-- .../scripts/mqtt_check_broker.sh | 40 +++++++ .../scripts/mqtt_common.sh | 46 ++++++++ .../scripts/mqtt_inspect.sh | 38 +++++++ .../scripts/mqtt_test_bridge.sh | 76 +++++++++++++ .../scripts/mqtt_test_pubsub.sh | 43 ++++++++ 9 files changed, 375 insertions(+), 18 deletions(-) create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_check_broker.sh create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_inspect.sh create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_bridge.sh create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_pubsub.sh diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore index fff2bf50..a14297b5 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore @@ -1 +1,2 @@ mqtt_message_bridge/__pycache__ +.mqtt_secrets.env diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml index 68b35661..46f07801 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml @@ -1,3 +1,39 @@ +mqtt: + host: "localhost" + port: 1883 + # protocol: mqtt # 'mqtt' (default, MQTTv311) or 'mqttv5' + # transport: tcp # 'tcp' (default) or 'websockets' + # keepalive: 60 + # reconnect_delay: 5 + # reconnect_max_delay: 120 + + # Each parameter above can be overridden by an environment variable. + # Set the *_env key to the name of the env var to read. + # The env var takes precedence over the plain value when both are set. + host_env: "MQTT_HOST" + port_env: "MQTT_PORT" + # protocol_env: "MQTT_PROTOCOL" + # transport_env: "MQTT_TRANSPORT" + # keepalive_env: "MQTT_KEEPALIVE" + # reconnect_delay_env: "MQTT_RECONNECT_DELAY" + # reconnect_max_delay_env: "MQTT_RECONNECT_MAX_DELAY" + + # Path to a .env file to load before resolving any env vars below. + # Variables already present in the environment take precedence. + # env_file: "/path/to/mqtt.env" + + # TLS settings (optional). + # tls: + # ca_certs: "/path/to/ca.crt" + # certfile: "/path/to/client.crt" + # keyfile: "/path/to/client.key" + # insecure: false # Set true to skip certificate verification. + + # Authentication (optional). + auth: + username_env: "MQTT_USERNAME" # Name of the env var holding the username. + password_env: "MQTT_PASSWORD" # Name of the env var holding the password. + # Bridge ROS2 DDS topics to MQTT. ros2_to_mqtt: - ros_topic: "/ros2_chatter" diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/launch/bridge.launch.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/launch/bridge.launch.py index e963645b..5173f16f 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/launch/bridge.launch.py +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/launch/bridge.launch.py @@ -1,22 +1,14 @@ from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration from ament_index_python.packages import get_package_share_directory import os def generate_launch_description(): config = os.path.join(get_package_share_directory('mqtt_message_bridge'), 'config', 'bridge_config.yaml') return LaunchDescription([ - DeclareLaunchArgument('mqtt_broker', default_value='localhost'), - DeclareLaunchArgument('mqtt_port', default_value='1883'), Node( package='mqtt_message_bridge', executable='bridge_node', - parameters=[{ - 'config_path': config, - 'mqtt_broker': LaunchConfiguration('mqtt_broker'), - 'mqtt_port': LaunchConfiguration('mqtt_port'), - }] + parameters=[{'config_path': config}] ) ]) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py index 3c112d75..d3172ea4 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py @@ -11,6 +11,11 @@ _STR_TYPE = 'std_msgs/msg/String' +_PROTOCOL_MAP = { + 'mqtt': mqtt.MQTTv311, + 'mqttv5': mqtt.MQTTv5, +} + def _serializer(ros_type: str, fmt: str): if fmt == 'json': return lambda msg, rt=ros_type: msg_to_json(msg, rt) @@ -50,8 +55,6 @@ class ROS2MQTTBridge(Node): def __init__(self): super().__init__('mqtt_bridge_node') self.declare_parameter('config_path', '') - self.declare_parameter('mqtt_broker', 'localhost') - self.declare_parameter('mqtt_port', 1883) config_path = self.get_parameter('config_path').get_parameter_value().string_value if not config_path or not os.path.exists(config_path): @@ -70,25 +73,107 @@ def __init__(self): self.ros_pubs = {} self._m2r_queue = queue.Queue() self._shutdown_event = threading.Event() - - # Map MQTT topic -> (ros_publisher, msg_type) for inbound routing self._mqtt_topic_map = {} + self._load_env_file(self.config.get('mqtt', {}).get('env_file')) self._setup_mqtt() self._setup_ros2_to_mqtt() self._setup_mqtt_to_ros2() self.mqtt_client.loop_start() self.create_timer(0.01, self._drain_m2r_queue) + def _load_env_file(self, env_file: str | None): + if not env_file: + return + if not os.path.exists(env_file): + self.get_logger().warning(f"env_file not found: {env_file}") + return + with open(env_file) as f: + for line in f: + line = line.strip() + if not line or line.startswith('#') or '=' not in line: + continue + key, _, value = line.partition('=') + key = key.strip() + value = value.strip().strip('"').strip("'") + os.environ.setdefault(key, value) + self.get_logger().info(f"Loaded env file: {env_file}") + + @staticmethod + def _env_or(cfg: dict, key: str, default=None): + env_var = cfg.get(f'{key}_env') + if env_var: + val = os.environ.get(env_var) + if val is not None: + return val + return cfg.get(key, default) + def _setup_mqtt(self): - broker = self.get_parameter('mqtt_broker').get_parameter_value().string_value - port = self.get_parameter('mqtt_port').get_parameter_value().integer_value + cfg = self.config.get('mqtt', {}) + + host = self._env_or(cfg, 'host', 'localhost') + port = int(self._env_or(cfg, 'port', 1883)) + keepalive = int(self._env_or(cfg, 'keepalive', 60)) + transport = self._env_or(cfg, 'transport', 'tcp') + protocol = _PROTOCOL_MAP.get(self._env_or(cfg, 'protocol', 'mqtt'), mqtt.MQTTv311) - self.mqtt_client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2) + self.mqtt_client = mqtt.Client( + mqtt.CallbackAPIVersion.VERSION2, + protocol=protocol, + transport=transport, + ) self.mqtt_client.on_connect = self._on_mqtt_connect self.mqtt_client.on_message = self._on_mqtt_message - self.mqtt_client.connect(broker, port) - self.get_logger().info(f'Connecting to MQTT broker: {broker}:{port}') + + self._configure_auth(cfg) + self._configure_tls(cfg) + self._configure_reconnect(cfg) + + self.mqtt_client.connect(host, port, keepalive=keepalive) + self.get_logger().info(f'Connecting to MQTT broker: {host}:{port}') + + def _configure_auth(self, cfg: dict): + auth = cfg.get('auth') + if not auth: + return + + username_env = auth.get('username_env') + password_env = auth.get('password_env') + + username = os.environ.get(username_env) if username_env else None + password = os.environ.get(password_env) if password_env else None + + if not username: + self.get_logger().warning( + f"MQTT auth enabled but env var '{username_env}' is not set or empty" + ) + return + + self.mqtt_client.username_pw_set(username, password) + self.get_logger().info(f"MQTT auth configured from env vars (user: '{username_env}')") + + def _configure_tls(self, cfg: dict): + tls = cfg.get('tls') + if not tls: + return + + self.mqtt_client.tls_set( + ca_certs=tls.get('ca_certs'), + certfile=tls.get('certfile'), + keyfile=tls.get('keyfile'), + ) + if tls.get('insecure', False): + self.mqtt_client.tls_insecure_set(True) + self.get_logger().warning('TLS certificate verification is disabled') + + def _configure_reconnect(self, cfg: dict): + delay = self._env_or(cfg, 'reconnect_delay') + max_delay = self._env_or(cfg, 'reconnect_max_delay') + if delay is not None or max_delay is not None: + self.mqtt_client.reconnect_delay_set( + min_delay=int(delay or 1), + max_delay=int(max_delay or 120), + ) def _on_mqtt_connect(self, client, userdata, flags, reason_code, properties): if reason_code == 0: diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_check_broker.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_check_broker.sh new file mode 100755 index 00000000..f3f092d3 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_check_broker.sh @@ -0,0 +1,40 @@ +#!/usr/bin/env bash +# Checks broker reachability, reports server info, and lists active subscriptions. +# +# Usage: ./mqtt_check_broker.sh [/path/to/mqtt.env] + +set -euo pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/mqtt_common.sh" "${1:-}" + +echo "=== MQTT Broker Check ===" +echo "Host : $MQTT_HOST" +echo "Port : $MQTT_PORT" +echo "User : ${MQTT_USERNAME:-}" +echo "" + +# TCP reachability +if ! nc -z -w3 "$MQTT_HOST" "$MQTT_PORT" 2>/dev/null; then + echo "FAIL: Cannot reach $MQTT_HOST:$MQTT_PORT" >&2 + exit 1 +fi +echo "TCP connection: OK" +echo "" + +# Broker info via $SYS topics (available on mosquitto and most brokers) +echo "=== Broker Info (sampling \$SYS for 3s) ===" +mosquitto_sub "${_broker_args[@]}" \ + -t '$SYS/broker/version' \ + -t '$SYS/broker/uptime' \ + -t '$SYS/broker/clients/connected' \ + -t '$SYS/broker/messages/received' \ + -t '$SYS/broker/messages/sent' \ + -t '$SYS/broker/subscriptions/count' \ + -W 3 -v 2>/dev/null || true + +echo "" +echo "=== Active Subscriptions (sampling \$SYS for 3s) ===" +mosquitto_sub "${_broker_args[@]}" \ + -t '$SYS/#' \ + -W 3 -v 2>/dev/null \ + | grep -i 'subscri' || echo "(none reported)" diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh new file mode 100755 index 00000000..95d291dd --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh @@ -0,0 +1,46 @@ +#!/usr/bin/env bash +# Sourced by other scripts. Reads broker settings from a .env file if provided, +# then falls back to environment variables, then to defaults. +# +# Usage: source mqtt_common.sh [/path/to/mqtt.env] + +_env_file="${1:-${MQTT_ENV_FILE:-}}" + +if [[ -n "$_env_file" ]]; then + if [[ ! -f "$_env_file" ]]; then + echo "ERROR: env file not found: $_env_file" >&2 + exit 1 + fi + set -a + # shellcheck disable=SC1090 + source "$_env_file" + set +a +fi + +MQTT_HOST="${MQTT_HOST:-localhost}" +MQTT_PORT="${MQTT_PORT:-1883}" +MQTT_USERNAME="${MQTT_USERNAME:-}" +MQTT_PASSWORD="${MQTT_PASSWORD:-}" +# TLS: set MQTT_TLS=1 to enable. Provide MQTT_CA_CERT for a custom CA, +# or leave unset to use the system CA store (/etc/ssl/certs). +MQTT_TLS="${MQTT_TLS:-}" +MQTT_CA_CERT="${MQTT_CA_CERT:-}" + +_auth_args=() +if [[ -n "$MQTT_USERNAME" ]]; then + _auth_args+=(-u "$MQTT_USERNAME") +fi +if [[ -n "$MQTT_PASSWORD" ]]; then + _auth_args+=(-P "$MQTT_PASSWORD") +fi + +_tls_args=() +if [[ -n "$MQTT_TLS" ]]; then + if [[ -n "$MQTT_CA_CERT" ]]; then + _tls_args+=(--cafile "$MQTT_CA_CERT") + else + _tls_args+=(--capath /etc/ssl/certs) + fi +fi + +_broker_args=(-h "$MQTT_HOST" -p "$MQTT_PORT" "${_auth_args[@]}" "${_tls_args[@]}") diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_inspect.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_inspect.sh new file mode 100755 index 00000000..530c2a49 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_inspect.sh @@ -0,0 +1,38 @@ +#!/usr/bin/env bash +# Subscribes to one or more topics and pretty-prints incoming messages with timestamps. +# Defaults to '#' (all topics) if no topic filter is given. +# +# Usage: ./mqtt_inspect.sh [/path/to/mqtt.env] [topic1] [topic2] ... +# +# Examples: +# ./mqtt_inspect.sh # all topics +# ./mqtt_inspect.sh mqtt.env ros2/# # all ROS2 bridge topics +# ./mqtt_inspect.sh mqtt.env ros2/# mqtt/# # two filters + +set -euo pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/mqtt_common.sh" "${1:-}" +shift || true # drop the env file arg (or nothing) so remaining args are topics + +TOPICS=("${@}") +if [[ ${#TOPICS[@]} -eq 0 ]]; then + TOPICS=('#') +fi + +_topic_args=() +for t in "${TOPICS[@]}"; do + _topic_args+=(-t "$t") +done + +echo "=== MQTT Inspector ===" +echo "Broker : $MQTT_HOST:$MQTT_PORT" +echo "Topics : ${TOPICS[*]}" +echo "Press Ctrl+C to stop." +echo "" + +mosquitto_sub "${_broker_args[@]}" \ + "${_topic_args[@]}" \ + -v \ + | while IFS= read -r line; do + echo "[$(date '+%H:%M:%S.%3N')] $line" + done diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_bridge.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_bridge.sh new file mode 100755 index 00000000..b2997b0b --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_bridge.sh @@ -0,0 +1,76 @@ +#!/usr/bin/env bash +# Exercises the bridge topic mappings defined in a bridge_config.yaml. +# For each ros2_to_mqtt mapping: publishes on the MQTT topic and listens for it. +# For each mqtt_to_ros2 mapping: same in the other direction. +# +# Usage: ./mqtt_test_bridge.sh [/path/to/mqtt.env] [/path/to/bridge_config.yaml] + +set -euo pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/mqtt_common.sh" "${1:-}" + +CONFIG="${2:-$(dirname "$SCRIPT_DIR")/config/bridge_config.yaml}" +TIMEOUT=5 +PASS=0 +FAIL=0 + +if [[ ! -f "$CONFIG" ]]; then + echo "ERROR: bridge config not found: $CONFIG" >&2 + exit 1 +fi + +echo "=== MQTT Bridge Topic Test ===" +echo "Broker : $MQTT_HOST:$MQTT_PORT" +echo "Config : $CONFIG" +echo "" + +_test_topic() { + local direction="$1" + local topic="$2" + local payload="test-$(date +%s%N)" + local tmpfile + tmpfile=$(mktemp) + + mosquitto_sub "${_broker_args[@]}" -t "$topic" -C 1 -W "$TIMEOUT" \ + > "$tmpfile" 2>/dev/null & + local sub_pid=$! + sleep 0.3 + + mosquitto_pub "${_broker_args[@]}" -t "$topic" -m "$payload" + + if wait "$sub_pid" 2>/dev/null && [[ "$(cat "$tmpfile")" == "$payload" ]]; then + echo " PASS [$direction] $topic" + (( PASS++ )) || true + else + echo " FAIL [$direction] $topic" >&2 + (( FAIL++ )) || true + fi + rm -f "$tmpfile" +} + +# Parse YAML with awk - extract mqtt_topic values from each section +_extract_mqtt_topics() { + local section="$1" + awk " + /^${section}:/ { in_section=1; next } + /^[a-z]/ && !/^${section}:/ { in_section=0 } + in_section && /mqtt_topic:/ { gsub(/.*mqtt_topic:[[:space:]]*\"|\"/, \"\"); print } + " "$CONFIG" +} + +echo "--- ros2_to_mqtt topics ---" +while IFS= read -r topic; do + [[ -z "$topic" ]] && continue + _test_topic "ros2->mqtt" "$topic" +done < <(_extract_mqtt_topics "ros2_to_mqtt") + +echo "" +echo "--- mqtt_to_ros2 topics ---" +while IFS= read -r topic; do + [[ -z "$topic" ]] && continue + _test_topic "mqtt->ros2" "$topic" +done < <(_extract_mqtt_topics "mqtt_to_ros2") + +echo "" +echo "Results: $PASS passed, $FAIL failed" +[[ $FAIL -eq 0 ]] diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_pubsub.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_pubsub.sh new file mode 100755 index 00000000..70fcac0f --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_pubsub.sh @@ -0,0 +1,43 @@ +#!/usr/bin/env bash +# Publishes a test message and verifies it is received back on the same topic. +# Exits 0 on success, 1 on timeout or mismatch. +# +# Usage: ./mqtt_test_pubsub.sh [/path/to/mqtt.env] [topic] [message] + +set -euo pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/mqtt_common.sh" "${1:-}" + +TOPIC="${2:-mqtt/bridge/test}" +PAYLOAD="${3:-ping-$(date +%s)}" +TIMEOUT=5 + +echo "=== MQTT Pub/Sub Round-Trip Test ===" +echo "Broker : $MQTT_HOST:$MQTT_PORT" +echo "Topic : $TOPIC" +echo "Payload : $PAYLOAD" +echo "" + +# Subscribe in the background, write received message to a temp file +_tmpfile=$(mktemp) +trap 'rm -f "$_tmpfile"' EXIT + +mosquitto_sub "${_broker_args[@]}" \ + -t "$TOPIC" -C 1 -W "$TIMEOUT" \ + > "$_tmpfile" 2>/dev/null & +_sub_pid=$! + +sleep 0.3 # Give the subscriber time to connect + +mosquitto_pub "${_broker_args[@]}" \ + -t "$TOPIC" -m "$PAYLOAD" + +wait "$_sub_pid" 2>/dev/null +_received=$(cat "$_tmpfile") + +if [[ "$_received" == "$PAYLOAD" ]]; then + echo "PASS: received '$_received'" +else + echo "FAIL: expected '$PAYLOAD', got '${_received:-}'" >&2 + exit 1 +fi From e5cbd9b8ebc05bc591ae6491179270f3ae4aa111 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Wed, 17 Jun 2026 08:54:20 +0200 Subject: [PATCH 03/18] added grpc_message_bridge Signed-off-by: Andrew Koerner --- .../grpc_message_bridge/Dockerfile | 31 ++ .../grpc_message_bridge/Makefile | 71 +++++ .../config/bridge_config.yaml | 41 +++ .../grpc_message_bridge/docker-entrypoint.sh | 11 + .../grpc_message_bridge/__init__.py | 0 .../grpc_message_bridge/bridge_node.py | 292 ++++++++++++++++++ .../grpc_message_bridge/ros_bridge.proto | 28 ++ .../grpc_message_bridge/ros_bridge_pb2.py | 42 +++ .../ros_bridge_pb2_grpc.py | 140 +++++++++ .../grpc_message_bridge/utils.py | 60 ++++ .../grpc_message_bridge/grpc_publish.py | 34 ++ .../grpc_message_bridge/grpc_server.py | 90 ++++++ .../grpc_message_bridge/grpc_subscribe.py | 24 ++ .../launch/bridge.launch.py | 24 ++ .../grpc_message_bridge/package.xml | 16 + .../grpc_message_bridge/requirements.pip3 | 4 + .../grpc_message_bridge/requirements.system | 3 + .../resource/grpc_message_bridge | 0 .../scripts/grpc_check_server.sh | 29 ++ .../scripts/grpc_common.sh | 22 ++ .../scripts/grpc_test_bridge.sh | 74 +++++ .../scripts/grpc_test_pubsub.sh | 56 ++++ .../grpc_message_bridge/setup.cfg | 8 + .../grpc_message_bridge/setup.py | 25 ++ .../start_grpc_message_bridge.sh | 17 + .../start_local_grpc_server.sh | 19 ++ .../stop_local_grpc_server.sh | 20 ++ 27 files changed, 1181 insertions(+) create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/Dockerfile create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml create mode 100755 ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-entrypoint.sh create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/__init__.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge.proto create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge_pb2.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge_pb2_grpc.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/utils.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_publish.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_server.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_subscribe.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/launch/bridge.launch.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/package.xml create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/requirements.pip3 create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/requirements.system create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/resource/grpc_message_bridge create mode 100755 ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_check_server.sh create mode 100755 ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_common.sh create mode 100755 ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_bridge.sh create mode 100755 ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_pubsub.sh create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.cfg create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py create mode 100755 ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_grpc_message_bridge.sh create mode 100755 ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_local_grpc_server.sh create mode 100755 ros2_workspace/src/adore_interfaces/grpc_message_bridge/stop_local_grpc_server.sh diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Dockerfile b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Dockerfile new file mode 100644 index 00000000..7b58f6a5 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Dockerfile @@ -0,0 +1,31 @@ +ARG ROS_DISTRO=jazzy +ARG OS_CODE_NAME=noble +FROM ros:${ROS_DISTRO}-ros-core-${OS_CODE_NAME} AS base +ARG ROS_DISTRO=jazzy +ENV ROS_DISTRO=${ROS_DISTRO} +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3-pip \ + python3-colcon-common-extensions \ + ros-${ROS_DISTRO}-rosidl-runtime-py \ + ros-${ROS_DISTRO}-std-msgs \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /ros2_ws/src/grpc_message_bridge + +COPY . . + +RUN pip3 install --no-cache-dir --break-system-packages -r requirements.pip3 + +WORKDIR /ros2_ws + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \ + colcon build --packages-select grpc_message_bridge --symlink-install + +ENV RMW_IMPLEMENTATION=rmw_fastrtps_cpp + +COPY docker-entrypoint.sh /docker-entrypoint.sh +RUN chmod +x /docker-entrypoint.sh + +ENTRYPOINT ["/docker-entrypoint.sh"] diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile new file mode 100644 index 00000000..4e17af90 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile @@ -0,0 +1,71 @@ +IMAGE_NAME ?= grpc_message_bridge +IMAGE_TAG ?= latest +ROS_DISTRO ?= jazzy +OS_CODE_NAME ?= noble + +BRIDGE_CONFIG ?= $(PWD)/config/bridge_config.yaml + +GRPC_HOST ?= 0.0.0.0 +GRPC_PORT ?= 50051 + +.PHONY: help +help: + @echo "Usage: make " + @echo "" + @echo "Targets:" + @echo " build Build the bridge Docker image" + @echo " run Start the bridge node container" + @echo " stop Stop and remove running containers" + @echo " logs Tail bridge container logs" + @echo " clean Stop containers, remove image and prune build cache" + @echo " gen_proto Regenerate Python stubs from ros_bridge.proto" + @echo "" + @echo "Overridable variables:" + @echo " IMAGE_NAME $(IMAGE_NAME)" + @echo " IMAGE_TAG $(IMAGE_TAG)" + @echo " ROS_DISTRO $(ROS_DISTRO)" + @echo " OS_CODE_NAME $(OS_CODE_NAME)" + @echo " BRIDGE_CONFIG $(BRIDGE_CONFIG)" + @echo " GRPC_HOST $(GRPC_HOST)" + @echo " GRPC_PORT $(GRPC_PORT)" + +.PHONY: build +build: + docker build \ + --build-arg ROS_DISTRO=$(ROS_DISTRO) \ + --build-arg OS_CODE_NAME=$(OS_CODE_NAME) \ + -t $(IMAGE_NAME):$(IMAGE_TAG) . + +.PHONY: run +run: + docker run --rm -d \ + --name grpc-bridge \ + --network host \ + -v $(BRIDGE_CONFIG):/config/bridge_config.yaml:ro \ + -e BRIDGE_CONFIG_PATH=/config/bridge_config.yaml \ + -e GRPC_HOST=$(GRPC_HOST) \ + -e GRPC_PORT=$(GRPC_PORT) \ + $(IMAGE_NAME):$(IMAGE_TAG) + +.PHONY: stop +stop: + docker stop grpc-bridge 2>/dev/null || true + +.PHONY: logs +logs: + docker logs -f grpc-bridge + +.PHONY: clean +clean: stop + docker rmi $(IMAGE_NAME):$(IMAGE_TAG) 2>/dev/null || true + docker builder prune -f + +.PHONY: gen_proto +gen_proto: + python3 -m grpc_tools.protoc \ + -I grpc_message_bridge \ + --python_out=grpc_message_bridge \ + --grpc_python_out=grpc_message_bridge \ + grpc_message_bridge/ros_bridge.proto + sed -i 's/^import ros_bridge_pb2/from . import ros_bridge_pb2/' \ + grpc_message_bridge/ros_bridge_pb2_grpc.py diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml new file mode 100644 index 00000000..8f39a4a8 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml @@ -0,0 +1,41 @@ +# ROS2 domain ID this bridge node subscribes/publishes on. +# Defaults to $ROS_DOMAIN if unset. +ros_domain_id: 0 + +# Bridge ROS2 topics to a remote gRPC server. +# The remote end must be running a grpc_message_bridge with grpc_to_ros2 configured +# for the same topic. +ros2_to_grpc: + - ros_topic: "/ros2_chatter" + msg_type: "std_msgs/msg/String" + grpc_address: "localhost:50052" # Remote bridge gRPC server address + # format: cdr # 'cdr' (default): raw CDR bytes. + # # 'json': raw JSON bytes with 'datatype' metadata. + # # Bridge-to-bridge only. + # # 'cdr_json': CDR-serialized std_msgs/msg/String whose + # # 'data' field contains JSON with 'datatype' metadata. + # qos_reliability: reliable # 'reliable' (default) or 'best_effort' + # qos_durability: volatile # 'volatile' (default) or 'transient_local' + # qos_depth: 1 + +# Accept inbound gRPC Publish streams and forward to ROS2 topics. +# The sending end must be running a grpc_message_bridge with ros2_to_grpc configured +# for the same topic pointing at this node's grpc_port. +grpc_to_ros2: + - ros_topic: "/grpc_chatter" + msg_type: "std_msgs/msg/String" + # format: cdr + # qos_reliability: best_effort # 'best_effort' (default) or 'reliable' + # qos_durability: volatile + # qos_depth: 1 + +# Connect as a gRPC client subscriber to a remote bridge server and publish +# the received messages locally on ROS2. Complements ros2_to_grpc on the remote side. +# grpc_subscribe_from: +# - ros_topic: "/remote_chatter" +# msg_type: "std_msgs/msg/String" +# grpc_address: "localhost:50052" +# # format: cdr +# # qos_reliability: best_effort +# # qos_durability: volatile +# # qos_depth: 1 diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-entrypoint.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-entrypoint.sh new file mode 100755 index 00000000..0a43e754 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-entrypoint.sh @@ -0,0 +1,11 @@ +#!/bin/bash +set -e + +source /opt/ros/${ROS_DISTRO}/setup.bash +source /ros2_ws/install/setup.bash + +exec ros2 run grpc_message_bridge bridge_node \ + --ros-args \ + -p config_path:="${BRIDGE_CONFIG_PATH:-/ros2_ws/install/grpc_message_bridge/share/grpc_message_bridge/config/bridge_config.yaml}" \ + -p grpc_host:="${GRPC_HOST:-0.0.0.0}" \ + -p grpc_port:="${GRPC_PORT:-50051}" diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/__init__.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py new file mode 100644 index 00000000..058de201 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py @@ -0,0 +1,292 @@ +import os +import queue +import threading +import time +import yaml +import grpc +import rclpy +from concurrent import futures +from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy +from . import ros_bridge_pb2, ros_bridge_pb2_grpc +from .utils import load_msg_type, msg_to_bytes, bytes_to_msg, msg_to_json, json_to_msg, msg_to_cdr_json, cdr_json_to_msg + +_STR_TYPE = 'std_msgs/msg/String' + +_DURABILITY = { + 'volatile': DurabilityPolicy.VOLATILE, + 'transient_local': DurabilityPolicy.TRANSIENT_LOCAL, +} +_RELIABILITY = { + 'best_effort': ReliabilityPolicy.BEST_EFFORT, + 'reliable': ReliabilityPolicy.RELIABLE, +} + + +def _wire_type(ros_type: str, fmt: str) -> str: + return _STR_TYPE if fmt in ('json', 'cdr_json') else ros_type + + +def _serializer(ros_type: str, fmt: str): + if fmt == 'json': + return lambda msg, rt=ros_type: msg_to_json(msg, rt) + if fmt == 'cdr_json': + return lambda msg, rt=ros_type: msg_to_cdr_json(msg, rt) + return msg_to_bytes + + +def _deserializer(msg_type, fmt: str): + if fmt == 'json': + return lambda data, mt=msg_type: json_to_msg(data, mt) + if fmt == 'cdr_json': + return lambda data, mt=msg_type: cdr_json_to_msg(data, mt) + return lambda data, mt=msg_type: bytes_to_msg(data, mt) + + +def _qos_from_mapping(mapping: dict, default_reliability: str = 'reliable') -> QoSProfile: + return QoSProfile( + depth=mapping.get('qos_depth', 1), + durability=_DURABILITY.get(mapping.get('qos_durability', 'volatile'), DurabilityPolicy.VOLATILE), + reliability=_RELIABILITY.get(mapping.get('qos_reliability', default_reliability), + _RELIABILITY[default_reliability]), + history=HistoryPolicy.KEEP_LAST, + ) + + +class RosBridgeServicer(ros_bridge_pb2_grpc.RosBridgeServicer): + """gRPC servicer: handles inbound Publish streams and outbound Subscribe streams.""" + + def __init__(self, node: 'ROS2GrpcBridge'): + self._node = node + + def Publish(self, request_iterator, context): + """Receive a stream of RosMessages from a remote publisher and forward to ROS.""" + for grpc_msg in request_iterator: + if self._node._shutdown_event.is_set(): + break + topic = grpc_msg.topic + pub = self._node.ros_pubs.get(topic) + if pub is None: + self._node.get_logger().warn(f'Received publish for unconfigured topic: {topic}') + continue + deser = self._node._grpc_deserializers.get(topic) + if deser is None: + continue + try: + msg = deser(grpc_msg.payload) + self._node._grpc_to_ros_queue.put((pub, msg)) + except Exception as e: + self._node.get_logger().error(f'Deser failed on {topic}: {e}') + return ros_bridge_pb2.Ack(ok=True) + + def Subscribe(self, request, context): + """Stream RosMessages to a remote subscriber for a configured topic.""" + topic = request.topic + sub_queue = queue.Queue() + + with self._node._subscriber_lock: + self._node._grpc_subscribers.setdefault(topic, []).append(sub_queue) + + self._node.get_logger().info(f'gRPC subscriber connected: {topic}') + try: + while not self._node._shutdown_event.is_set() and context.is_active(): + try: + grpc_msg = sub_queue.get(timeout=0.1) + yield grpc_msg + except queue.Empty: + continue + finally: + with self._node._subscriber_lock: + subs = self._node._grpc_subscribers.get(topic, []) + if sub_queue in subs: + subs.remove(sub_queue) + self._node.get_logger().info(f'gRPC subscriber disconnected: {topic}') + + +class ROS2GrpcBridge(Node): + def __init__(self): + super().__init__('grpc_bridge_node') + self.declare_parameter('config_path', '') + self.declare_parameter('grpc_host', '0.0.0.0') + self.declare_parameter('grpc_port', 50051) + self.declare_parameter('grpc_server_address', '') + + config_path = self.get_parameter('config_path').get_parameter_value().string_value + if not config_path or not os.path.exists(config_path): + self.get_logger().error(f'Config file not found: {config_path}') + return + + try: + with open(config_path, 'r') as f: + self.config = yaml.safe_load(f) + except Exception as e: + self.get_logger().error(f'Failed to load config: {e}') + return + + self.ros_subs = [] + self.ros_pubs = {} + self._grpc_deserializers = {} + self._grpc_to_ros_queue = queue.Queue() + self._grpc_subscribers = {} + self._subscriber_lock = threading.Lock() + self._shutdown_event = threading.Event() + self._grpc_clients = {} + self._grpc_channels = {} + self._grpc_pub_streams = {} + + self._setup_grpc_server() + self._setup_ros2_to_grpc() + self._setup_grpc_to_ros2() + self.create_timer(0.01, self._drain_grpc_to_ros_queue) + + def _setup_grpc_server(self): + host = self.get_parameter('grpc_host').get_parameter_value().string_value + port = self.get_parameter('grpc_port').get_parameter_value().integer_value + self._grpc_server = grpc.server(futures.ThreadPoolExecutor(max_workers=10)) + ros_bridge_pb2_grpc.add_RosBridgeServicer_to_server(RosBridgeServicer(self), self._grpc_server) + listen_addr = f'{host}:{port}' + self._grpc_server.add_insecure_port(listen_addr) + self._grpc_server.start() + self.get_logger().info(f'gRPC server listening on {listen_addr}') + + def _get_client(self, address: str): + if address not in self._grpc_channels: + channel = grpc.insecure_channel(address) + self._grpc_channels[address] = channel + self._grpc_clients[address] = ros_bridge_pb2_grpc.RosBridgeStub(channel) + return self._grpc_clients[address] + + def _setup_ros2_to_grpc(self): + """Subscribe to ROS topics and stream messages to remote gRPC servers.""" + for mapping in self.config.get('ros2_to_grpc', []): + ros_topic = mapping['ros_topic'] + ros_type = mapping.get('msg_type', _STR_TYPE) + remote_addr = mapping.get('grpc_address', '') + fmt = mapping.get('format', 'cdr') + qos = _qos_from_mapping(mapping, default_reliability='reliable') + msg_type = load_msg_type(ros_type) + serialize = _serializer(ros_type, fmt) + seq = [0] + + if not remote_addr: + self.get_logger().error(f'ros2_to_grpc entry for {ros_topic} missing grpc_address') + continue + + # Stream queue feeding a persistent per-topic publisher thread. + stream_queue: queue.Queue = queue.Queue() + self._grpc_pub_streams[ros_topic] = stream_queue + + def _stream_worker(addr=remote_addr, topic=ros_topic, rtype=ros_type, f=fmt, sq=stream_queue): + while not self._shutdown_event.is_set(): + try: + stub = self._get_client(addr) + stub.Publish(iter(sq.get, None)) + except grpc.RpcError as e: + self.get_logger().warn(f'gRPC publish error on {topic}: {e.details()} -- retrying in 2s') + time.sleep(2) + + t = threading.Thread(target=_stream_worker, daemon=True) + t.start() + + self.get_logger().info(f'R2G: {ros_topic} -> {remote_addr} [{fmt}]') + + def cb(msg, sq=stream_queue, topic=ros_topic, rtype=ros_type, f=fmt, s=seq, ser=serialize): + grpc_msg = ros_bridge_pb2.RosMessage( + topic = topic, + ros_type = rtype, + format = f, + payload = ser(msg), + seq = s[0], + stamp_ns = time.time_ns(), + ) + s[0] += 1 + sq.put(grpc_msg) + self.get_logger().debug(f'R2G queued: {topic}') + + self.ros_subs.append(self.create_subscription(msg_type, ros_topic, cb, qos)) + + def _setup_grpc_to_ros2(self): + """For each grpc_to_ros2 entry, register a ROS publisher and accept inbound gRPC Publish calls.""" + for mapping in self.config.get('grpc_to_ros2', []): + ros_topic = mapping['ros_topic'] + ros_type = mapping.get('msg_type', _STR_TYPE) + fmt = mapping.get('format', 'cdr') + wtype = _wire_type(ros_type, fmt) + qos = _qos_from_mapping(mapping, default_reliability='best_effort') + pub_type = load_msg_type(wtype) + m_type = load_msg_type(ros_type) + + self.ros_pubs[ros_topic] = self.create_publisher(pub_type, ros_topic, qos) + self._grpc_deserializers[ros_topic] = _deserializer(m_type, fmt) + self.get_logger().info(f'G2R: registered inbound topic {ros_topic} [{fmt}]') + + def _setup_grpc_subscribe_forwarders(self): + """ + For ros2_to_grpc entries that also want a remote Subscribe channel, + connect as a gRPC client subscriber and publish received messages locally. + Used when the remote side is a server and this node is the consumer. + """ + for mapping in self.config.get('grpc_subscribe_from', []): + ros_topic = mapping['ros_topic'] + ros_type = mapping.get('msg_type', _STR_TYPE) + remote_addr = mapping['grpc_address'] + fmt = mapping.get('format', 'cdr') + qos = _qos_from_mapping(mapping, default_reliability='best_effort') + wtype = _wire_type(ros_type, fmt) + pub_type = load_msg_type(wtype) + m_type = load_msg_type(ros_type) + deserialize = _deserializer(m_type, fmt) + + pub = self.create_publisher(pub_type, ros_topic, qos) + self.ros_pubs[ros_topic] = pub + self.get_logger().info(f'GSF: {remote_addr} -> {ros_topic} [{fmt}]') + + def _sub_worker(addr=remote_addr, topic=ros_topic, rtype=ros_type, f=fmt, p=pub, deser=deserialize): + while not self._shutdown_event.is_set(): + try: + stub = self._get_client(addr) + req = ros_bridge_pb2.SubscribeRequest(topic=topic, ros_type=rtype, format=f) + for grpc_msg in stub.Subscribe(req): + if self._shutdown_event.is_set(): + break + try: + msg = deser(grpc_msg.payload) + self._grpc_to_ros_queue.put((p, msg)) + except Exception as e: + self.get_logger().error(f'Deser failed on {topic}: {e}') + except grpc.RpcError as e: + self.get_logger().warn(f'gRPC subscribe error on {topic}: {e.details()} -- retrying in 2s') + time.sleep(2) + + threading.Thread(target=_sub_worker, daemon=True).start() + + def _drain_grpc_to_ros_queue(self): + while not self._grpc_to_ros_queue.empty(): + pub, msg = self._grpc_to_ros_queue.get_nowait() + pub.publish(msg) + + def shutdown(self): + self._shutdown_event.set() + # Sentinel None unblocks iter(queue.get, None) in publisher threads. + for sq in self._grpc_pub_streams.values(): + sq.put(None) + self._grpc_server.stop(grace=2) + for ch in self._grpc_channels.values(): + ch.close() + + +def main(args=None): + rclpy.init(args=args) + node = ROS2GrpcBridge() + executor = MultiThreadedExecutor() + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.shutdown() + executor.shutdown() + node.destroy_node() + rclpy.shutdown() diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge.proto b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge.proto new file mode 100644 index 00000000..d03f2231 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge.proto @@ -0,0 +1,28 @@ +syntax = "proto3"; + +package ros_bridge; + +service RosBridge { + rpc Publish(stream RosMessage) returns (Ack); + rpc Subscribe(SubscribeRequest) returns (stream RosMessage); +} + +message RosMessage { + string topic = 1; + string ros_type = 2; + string format = 3; + bytes payload = 4; + int64 seq = 5; + int64 stamp_ns = 6; +} + +message SubscribeRequest { + string topic = 1; + string ros_type = 2; + string format = 3; +} + +message Ack { + bool ok = 1; + string message = 2; +} diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge_pb2.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge_pb2.py new file mode 100644 index 00000000..e8b96690 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge_pb2.py @@ -0,0 +1,42 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# NO CHECKED-IN PROTOBUF GENCODE +# source: ros_bridge.proto +# Protobuf Python Version: 6.33.5 +"""Generated protocol buffer code.""" +from google.protobuf import descriptor as _descriptor +from google.protobuf import descriptor_pool as _descriptor_pool +from google.protobuf import runtime_version as _runtime_version +from google.protobuf import symbol_database as _symbol_database +from google.protobuf.internal import builder as _builder +_runtime_version.ValidateProtobufRuntimeVersion( + _runtime_version.Domain.PUBLIC, + 6, + 33, + 5, + '', + 'ros_bridge.proto' +) +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x10ros_bridge.proto\x12\nros_bridge\"m\n\nRosMessage\x12\r\n\x05topic\x18\x01 \x01(\t\x12\x10\n\x08ros_type\x18\x02 \x01(\t\x12\x0e\n\x06\x66ormat\x18\x03 \x01(\t\x12\x0f\n\x07payload\x18\x04 \x01(\x0c\x12\x0b\n\x03seq\x18\x05 \x01(\x03\x12\x10\n\x08stamp_ns\x18\x06 \x01(\x03\"C\n\x10SubscribeRequest\x12\r\n\x05topic\x18\x01 \x01(\t\x12\x10\n\x08ros_type\x18\x02 \x01(\t\x12\x0e\n\x06\x66ormat\x18\x03 \x01(\t\"\"\n\x03\x41\x63k\x12\n\n\x02ok\x18\x01 \x01(\x08\x12\x0f\n\x07message\x18\x02 \x01(\t2\x86\x01\n\tRosBridge\x12\x34\n\x07Publish\x12\x16.ros_bridge.RosMessage\x1a\x0f.ros_bridge.Ack(\x01\x12\x43\n\tSubscribe\x12\x1c.ros_bridge.SubscribeRequest\x1a\x16.ros_bridge.RosMessage0\x01\x62\x06proto3') + +_globals = globals() +_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals) +_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'ros_bridge_pb2', _globals) +if not _descriptor._USE_C_DESCRIPTORS: + DESCRIPTOR._loaded_options = None + _globals['_ROSMESSAGE']._serialized_start=32 + _globals['_ROSMESSAGE']._serialized_end=141 + _globals['_SUBSCRIBEREQUEST']._serialized_start=143 + _globals['_SUBSCRIBEREQUEST']._serialized_end=210 + _globals['_ACK']._serialized_start=212 + _globals['_ACK']._serialized_end=246 + _globals['_ROSBRIDGE']._serialized_start=249 + _globals['_ROSBRIDGE']._serialized_end=383 +# @@protoc_insertion_point(module_scope) diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge_pb2_grpc.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge_pb2_grpc.py new file mode 100644 index 00000000..80d624a9 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/ros_bridge_pb2_grpc.py @@ -0,0 +1,140 @@ +# Generated by the gRPC Python protocol compiler plugin. DO NOT EDIT! +"""Client and server classes corresponding to protobuf-defined services.""" +import grpc +import warnings + +from . import ros_bridge_pb2 as ros__bridge__pb2 + +GRPC_GENERATED_VERSION = '1.81.0' +GRPC_VERSION = grpc.__version__ +_version_not_supported = False + +try: + from grpc._utilities import first_version_is_lower + _version_not_supported = first_version_is_lower(GRPC_VERSION, GRPC_GENERATED_VERSION) +except ImportError: + _version_not_supported = True + +if _version_not_supported: + raise RuntimeError( + f'The grpc package installed is at version {GRPC_VERSION},' + + ' but the generated code in ros_bridge_pb2_grpc.py depends on' + + f' grpcio>={GRPC_GENERATED_VERSION}.' + + f' Please upgrade your grpc module to grpcio>={GRPC_GENERATED_VERSION}' + + f' or downgrade your generated code using grpcio-tools<={GRPC_VERSION}.' + ) + + +class RosBridgeStub: + """Missing associated documentation comment in .proto file.""" + + def __init__(self, channel): + """Constructor. + + Args: + channel: A grpc.Channel. + """ + self.Publish = channel.stream_unary( + '/ros_bridge.RosBridge/Publish', + request_serializer=ros__bridge__pb2.RosMessage.SerializeToString, + response_deserializer=ros__bridge__pb2.Ack.FromString, + _registered_method=True) + self.Subscribe = channel.unary_stream( + '/ros_bridge.RosBridge/Subscribe', + request_serializer=ros__bridge__pb2.SubscribeRequest.SerializeToString, + response_deserializer=ros__bridge__pb2.RosMessage.FromString, + _registered_method=True) + + +class RosBridgeServicer: + """Missing associated documentation comment in .proto file.""" + + def Publish(self, request_iterator, context): + """Missing associated documentation comment in .proto file.""" + context.set_code(grpc.StatusCode.UNIMPLEMENTED) + context.set_details('Method not implemented!') + raise NotImplementedError('Method not implemented!') + + def Subscribe(self, request, context): + """Missing associated documentation comment in .proto file.""" + context.set_code(grpc.StatusCode.UNIMPLEMENTED) + context.set_details('Method not implemented!') + raise NotImplementedError('Method not implemented!') + + +def add_RosBridgeServicer_to_server(servicer, server): + rpc_method_handlers = { + 'Publish': grpc.stream_unary_rpc_method_handler( + servicer.Publish, + request_deserializer=ros__bridge__pb2.RosMessage.FromString, + response_serializer=ros__bridge__pb2.Ack.SerializeToString, + ), + 'Subscribe': grpc.unary_stream_rpc_method_handler( + servicer.Subscribe, + request_deserializer=ros__bridge__pb2.SubscribeRequest.FromString, + response_serializer=ros__bridge__pb2.RosMessage.SerializeToString, + ), + } + generic_handler = grpc.method_handlers_generic_handler( + 'ros_bridge.RosBridge', rpc_method_handlers) + server.add_generic_rpc_handlers((generic_handler,)) + server.add_registered_method_handlers('ros_bridge.RosBridge', rpc_method_handlers) + + + # This class is part of an EXPERIMENTAL API. +class RosBridge: + """Missing associated documentation comment in .proto file.""" + + @staticmethod + def Publish(request_iterator, + target, + options=(), + channel_credentials=None, + call_credentials=None, + insecure=False, + compression=None, + wait_for_ready=None, + timeout=None, + metadata=None): + return grpc.experimental.stream_unary( + request_iterator, + target, + '/ros_bridge.RosBridge/Publish', + ros__bridge__pb2.RosMessage.SerializeToString, + ros__bridge__pb2.Ack.FromString, + options, + channel_credentials, + insecure, + call_credentials, + compression, + wait_for_ready, + timeout, + metadata, + _registered_method=True) + + @staticmethod + def Subscribe(request, + target, + options=(), + channel_credentials=None, + call_credentials=None, + insecure=False, + compression=None, + wait_for_ready=None, + timeout=None, + metadata=None): + return grpc.experimental.unary_stream( + request, + target, + '/ros_bridge.RosBridge/Subscribe', + ros__bridge__pb2.SubscribeRequest.SerializeToString, + ros__bridge__pb2.RosMessage.FromString, + options, + channel_credentials, + insecure, + call_credentials, + compression, + wait_for_ready, + timeout, + metadata, + _registered_method=True) diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/utils.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/utils.py new file mode 100644 index 00000000..9fc97917 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/utils.py @@ -0,0 +1,60 @@ +import importlib +import json +from rclpy.serialization import serialize_message, deserialize_message +from rosidl_runtime_py import message_to_ordereddict, set_message_fields + + +def load_msg_type(msg_type_str: str): + try: + pkg, interface, name = msg_type_str.split('/') + except ValueError: + raise ValueError(f"Invalid msg_type '{msg_type_str}'. Expected 'pkg/msg/Name'.") + module = importlib.import_module(f'{pkg}.{interface}') + return getattr(module, name) + + +def msg_to_bytes(msg) -> bytes: + return serialize_message(msg) + + +def bytes_to_msg(data: bytes, msg_type): + return deserialize_message(data, msg_type) + + +def msg_to_json(msg, ros_type: str) -> bytes: + """Serialize to raw JSON bytes with datatype metadata. Bridge-to-bridge only.""" + obj = message_to_ordereddict(msg) + obj['datatype'] = ros_type + return json.dumps(obj).encode('utf-8') + + +def json_to_msg(data: bytes, msg_type): + """Deserialize raw JSON bytes to a ROS message, stripping metadata fields.""" + obj = json.loads(data.decode('utf-8')) + obj.pop('datatype', None) + obj.pop('topic', None) + msg = msg_type() + set_message_fields(msg, obj) + return msg + + +def msg_to_cdr_json(msg, ros_type: str) -> bytes: + """Serialize to CDR-encoded std_msgs/msg/String whose data field is JSON with datatype metadata.""" + from std_msgs.msg import String + obj = message_to_ordereddict(msg) + obj['datatype'] = ros_type + wrapper = String() + wrapper.data = json.dumps(obj) + return serialize_message(wrapper) + + +def cdr_json_to_msg(data: bytes, msg_type): + """Deserialize a CDR std_msgs/msg/String containing JSON back into msg_type.""" + from std_msgs.msg import String + wrapper = deserialize_message(data, String) + obj = json.loads(wrapper.data) + obj.pop('datatype', None) + obj.pop('topic', None) + msg = msg_type() + set_message_fields(msg, obj) + return msg diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_publish.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_publish.py new file mode 100644 index 00000000..36d5eb55 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_publish.py @@ -0,0 +1,34 @@ +"""Test client: streams RosMessages to a running grpc_message_bridge server.""" +import argparse +import grpc +import json +import time +from grpc_message_bridge import ros_bridge_pb2, ros_bridge_pb2_grpc + +parser = argparse.ArgumentParser() +parser.add_argument('--address', default='localhost:50051') +parser.add_argument('--topic', default='/grpc_chatter') +parser.add_argument('--count', type=int, default=0, help='0 = infinite') +args = parser.parse_args() + +def message_stream(): + seq = 0 + while args.count == 0 or seq < args.count: + payload = json.dumps({'data': f'Hello gRPC #{seq}'}).encode() + yield ros_bridge_pb2.RosMessage( + topic = args.topic, + ros_type = 'std_msgs/msg/String', + format = 'json', + payload = payload, + seq = seq, + stamp_ns = time.time_ns(), + ) + print(f'Published seq={seq}') + seq += 1 + if args.count == 0: + time.sleep(1) + +with grpc.insecure_channel(args.address) as channel: + stub = ros_bridge_pb2_grpc.RosBridgeStub(channel) + ack = stub.Publish(message_stream()) + print(f'Ack: {ack}') diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_server.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_server.py new file mode 100644 index 00000000..58f39bf8 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_server.py @@ -0,0 +1,90 @@ +#!/usr/bin/env python3 +""" +Standalone local gRPC server implementing the RosBridge service. +No ROS required. Published messages are fanned out to all active +subscribers on the same topic. + +Usage: + python3 grpc_server.py [--host 0.0.0.0] [--port 50051] +""" +import argparse +import collections +import logging +import queue +import signal +import sys +import threading +from concurrent import futures + +import grpc + +from grpc_message_bridge import ros_bridge_pb2, ros_bridge_pb2_grpc + +logging.basicConfig(level=logging.INFO, format='%(asctime)s %(levelname)s %(message)s') +log = logging.getLogger(__name__) + + +class RosBridgeServicer(ros_bridge_pb2_grpc.RosBridgeServicer): + def __init__(self): + self._lock = threading.Lock() + self._subscribers = collections.defaultdict(list) # topic -> [Queue] + + def Publish(self, request_iterator, context): + for msg in request_iterator: + if not context.is_active(): + break + log.info('publish topic=%s seq=%d', msg.topic, msg.seq) + with self._lock: + queues = list(self._subscribers.get(msg.topic, [])) + for q in queues: + try: + q.put_nowait(msg) + except queue.Full: + pass + return ros_bridge_pb2.Ack(ok=True) + + def Subscribe(self, request, context): + q = queue.Queue(maxsize=256) + topic = request.topic + with self._lock: + self._subscribers[topic].append(q) + log.info('subscriber connected topic=%s', topic) + try: + while context.is_active(): + try: + msg = q.get(timeout=0.1) + yield msg + except queue.Empty: + continue + finally: + with self._lock: + subs = self._subscribers.get(topic, []) + if q in subs: + subs.remove(q) + log.info('subscriber disconnected topic=%s', topic) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('--host', default='0.0.0.0') + parser.add_argument('--port', type=int, default=50051) + args = parser.parse_args() + + address = f'{args.host}:{args.port}' + server = grpc.server(futures.ThreadPoolExecutor(max_workers=10)) + ros_bridge_pb2_grpc.add_RosBridgeServicer_to_server(RosBridgeServicer(), server) + server.add_insecure_port(address) + server.start() + log.info('gRPC server listening on %s', address) + + stop = threading.Event() + signal.signal(signal.SIGINT, lambda *_: stop.set()) + signal.signal(signal.SIGTERM, lambda *_: stop.set()) + stop.wait() + + log.info('shutting down') + server.stop(grace=2) + + +if __name__ == '__main__': + main() diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_subscribe.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_subscribe.py new file mode 100644 index 00000000..d8692ed8 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_subscribe.py @@ -0,0 +1,24 @@ +"""Test client: subscribes to a topic stream from a running grpc_message_bridge server.""" +import argparse +import grpc +from grpc_message_bridge import ros_bridge_pb2, ros_bridge_pb2_grpc + +parser = argparse.ArgumentParser() +parser.add_argument('--address', default='localhost:50051') +parser.add_argument('--topic', default='/ros2_chatter') +parser.add_argument('--count', type=int, default=0, help='0 = infinite') +args = parser.parse_args() + +with grpc.insecure_channel(args.address) as channel: + stub = ros_bridge_pb2_grpc.RosBridgeStub(channel) + req = ros_bridge_pb2.SubscribeRequest( + topic = args.topic, + ros_type = 'std_msgs/msg/String', + format = 'cdr', + ) + received = 0 + for msg in stub.Subscribe(req): + print(f'[{msg.topic}] seq={msg.seq} payload={msg.payload!r}') + received += 1 + if args.count and received >= args.count: + break diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/launch/bridge.launch.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/launch/bridge.launch.py new file mode 100644 index 00000000..070252f1 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/launch/bridge.launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + pkg_share = get_package_share_directory('grpc_message_bridge') + bridge_config = os.path.join(pkg_share, 'config', 'bridge_config.yaml') + + return LaunchDescription([ + DeclareLaunchArgument('grpc_host', default_value='0.0.0.0'), + DeclareLaunchArgument('grpc_port', default_value='50051'), + Node( + package='grpc_message_bridge', + executable='bridge_node', + parameters=[{ + 'config_path': bridge_config, + 'grpc_host': LaunchConfiguration('grpc_host'), + 'grpc_port': LaunchConfiguration('grpc_port'), + }] + ) + ]) diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/package.xml b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/package.xml new file mode 100644 index 00000000..ad673254 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/package.xml @@ -0,0 +1,16 @@ + + + grpc_message_bridge + 1.0.0 + Bridge between ROS 2 and gRPC + Eclipse ADORe + Apache-2.0 + rclpy + std_msgs + rosidl_runtime_py + grpcio + grpcio-tools + + ament_python + + diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/requirements.pip3 b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/requirements.pip3 new file mode 100644 index 00000000..b1385fc9 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/requirements.pip3 @@ -0,0 +1,4 @@ +grpcio>=1.60.0 +grpcio-tools>=1.60.0 +pyyaml +numpy diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/requirements.system b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/requirements.system new file mode 100644 index 00000000..de16f5ce --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/requirements.system @@ -0,0 +1,3 @@ +netcat-openbsd +python3-pip +protobuf-compiler diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/resource/grpc_message_bridge b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/resource/grpc_message_bridge new file mode 100644 index 00000000..e69de29b diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_check_server.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_check_server.sh new file mode 100755 index 00000000..c35ef4e7 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_check_server.sh @@ -0,0 +1,29 @@ +#!/usr/bin/env bash +# Checks gRPC server reachability and reports connection status. +# +# Usage: ./grpc_check_server.sh [/path/to/grpc.env] + +set -euo pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +source "$SCRIPT_DIR/grpc_common.sh" "${1:-}" + +echo "=== gRPC Server Check ===" +echo "Host : $GRPC_HOST" +echo "Port : $GRPC_PORT" +echo "" + +if ! nc -z -w3 "$GRPC_HOST" "$GRPC_PORT" 2>/dev/null; then + echo "FAIL: Cannot reach $GRPC_HOST:$GRPC_PORT" >&2 + exit 1 +fi +echo "TCP connection: OK" +echo "" + +# grpc_health_probe is available if grpcio-health-checking is installed +if command -v grpc_health_probe &>/dev/null; then + echo "=== gRPC Health Probe ===" + grpc_health_probe -addr="$GRPC_ADDRESS" && echo "Health: SERVING" || echo "Health: NOT SERVING" +else + echo "(grpc_health_probe not available -- TCP reachability confirmed above)" + echo "Install: pip3 install grpcio-health-checking" +fi diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_common.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_common.sh new file mode 100755 index 00000000..2aca10b0 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_common.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +# Sourced by other scripts. Reads gRPC settings from an .env file if provided, +# then falls back to environment variables, then to defaults. +# +# Usage: source grpc_common.sh [/path/to/grpc.env] + +_env_file="${1:-${GRPC_ENV_FILE:-}}" + +if [[ -n "$_env_file" ]]; then + if [[ ! -f "$_env_file" ]]; then + echo "ERROR: env file not found: $_env_file" >&2 + exit 1 + fi + set -a + # shellcheck disable=SC1090 + source "$_env_file" + set +a +fi + +GRPC_HOST="${GRPC_HOST:-localhost}" +GRPC_PORT="${GRPC_PORT:-50051}" +GRPC_ADDRESS="${GRPC_HOST}:${GRPC_PORT}" diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_bridge.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_bridge.sh new file mode 100755 index 00000000..b22f3f2b --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_bridge.sh @@ -0,0 +1,74 @@ +#!/usr/bin/env bash +# Exercises the bridge topic mappings defined in bridge_config.yaml. +# For each ros2_to_grpc and grpc_to_ros2 mapping: verifies the gRPC server +# accepts a publish stream on that topic without error. +# +# Usage: ./grpc_test_bridge.sh [/path/to/grpc.env] [/path/to/bridge_config.yaml] + +set -euo pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PACKAGE_DIR="$(dirname "$SCRIPT_DIR")" +source "$SCRIPT_DIR/grpc_common.sh" "${1:-}" + +CONFIG="${2:-$PACKAGE_DIR/config/bridge_config.yaml}" +TIMEOUT=5 +PASS=0 +FAIL=0 + +if [[ ! -f "$CONFIG" ]]; then + echo "ERROR: bridge config not found: $CONFIG" >&2 + exit 1 +fi + +echo "=== gRPC Bridge Topic Test ===" +echo "Server : $GRPC_ADDRESS" +echo "Config : $CONFIG" +echo "" + +_extract_topics() { + local section="$1" + awk " + /^${section}:/ { in_section=1; next } + /^[a-z]/ && !/^${section}:/ { in_section=0 } + in_section && /ros_topic:/ { gsub(/.*ros_topic:[[:space:]]*\"?|\"?[[:space:]]*$/, \"\"); print } + " "$CONFIG" +} + +_test_topic() { + local direction="$1" + local topic="$2" + local _tmpfile + _tmpfile=$(mktemp) + + python3 "$PACKAGE_DIR/grpc_publish.py" \ + --address "$GRPC_ADDRESS" --topic "$topic" --count 1 \ + > "$_tmpfile" 2>&1 + local rc=$? + + if [[ $rc -eq 0 ]] && grep -q "Published" "$_tmpfile"; then + echo " PASS [$direction] $topic" + (( PASS++ )) || true + else + echo " FAIL [$direction] $topic" >&2 + cat "$_tmpfile" >&2 + (( FAIL++ )) || true + fi + rm -f "$_tmpfile" +} + +echo "--- ros2_to_grpc topics ---" +while IFS= read -r topic; do + [[ -z "$topic" ]] && continue + _test_topic "ros2->grpc" "$topic" +done < <(_extract_topics "ros2_to_grpc") + +echo "" +echo "--- grpc_to_ros2 topics ---" +while IFS= read -r topic; do + [[ -z "$topic" ]] && continue + _test_topic "grpc->ros2" "$topic" +done < <(_extract_topics "grpc_to_ros2") + +echo "" +echo "Results: $PASS passed, $FAIL failed" +[[ $FAIL -eq 0 ]] diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_pubsub.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_pubsub.sh new file mode 100755 index 00000000..d10ee5a9 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_pubsub.sh @@ -0,0 +1,56 @@ +#!/usr/bin/env bash +# Spawns a grpc_subscribe.py listener then a grpc_publish.py sender and verifies +# at least one message is received. Exits 0 on success, 1 on timeout or error. +# +# Usage: ./grpc_test_pubsub.sh [/path/to/grpc.env] [topic] [timeout_seconds] + +set -euo pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PACKAGE_DIR="$(dirname "$SCRIPT_DIR")" +source "$SCRIPT_DIR/grpc_common.sh" "${1:-}" + +TOPIC="${2:-/grpc_chatter}" +TIMEOUT="${3:-8}" + +echo "=== gRPC Pub/Sub Round-Trip Test ===" +echo "Server : $GRPC_ADDRESS" +echo "Topic : $TOPIC" +echo "Timeout : ${TIMEOUT}s" +echo "" + +_tmpfile=$(mktemp) +trap 'rm -f "$_tmpfile"; kill "$_sub_pid" 2>/dev/null || true' EXIT + +GRPC_HOST="$GRPC_HOST" GRPC_PORT="$GRPC_PORT" \ + python3 "$PACKAGE_DIR/grpc_subscribe.py" --address "$GRPC_ADDRESS" --topic "$TOPIC" --count 1 \ + > "$_tmpfile" 2>&1 & +_sub_pid=$! + +sleep 0.5 + +GRPC_HOST="$GRPC_HOST" GRPC_PORT="$GRPC_PORT" \ + python3 "$PACKAGE_DIR/grpc_publish.py" --address "$GRPC_ADDRESS" --topic "$TOPIC" --count 1 \ + >> "$_tmpfile" 2>&1 & +_pub_pid=$! + +_deadline=$(( SECONDS + TIMEOUT )) +while kill -0 "$_sub_pid" 2>/dev/null; do + if (( SECONDS >= _deadline )); then + echo "FAIL: timed out after ${TIMEOUT}s" >&2 + cat "$_tmpfile" >&2 + exit 1 + fi + sleep 0.2 +done + +wait "$_sub_pid" 2>/dev/null +kill "$_pub_pid" 2>/dev/null || true + +if grep -q "seq=" "$_tmpfile"; then + echo "PASS: message received" + grep "seq=" "$_tmpfile" +else + echo "FAIL: no message received" >&2 + cat "$_tmpfile" >&2 + exit 1 +fi diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.cfg b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.cfg new file mode 100644 index 00000000..29fcd375 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.cfg @@ -0,0 +1,8 @@ +[metadata] +name = grpc_message_bridge + +[develop] +script_dir=$base/lib/grpc_message_bridge + +[install] +install_scripts=$base/lib/grpc_message_bridge diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py new file mode 100644 index 00000000..f2b748fb --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py @@ -0,0 +1,25 @@ +from setuptools import setup + +package_name = 'grpc_message_bridge' + +setup( + name=package_name, + version='1.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/config', ['config/bridge_config.yaml']), + ('share/' + package_name + '/launch', ['launch/bridge.launch.py']), + ], + install_requires=['setuptools', 'grpcio', 'grpcio-tools', 'pyyaml'], + zip_safe=True, + maintainer='akoerner', + description='ROS 2 gRPC Message Bridge', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'bridge_node = grpc_message_bridge.bridge_node:main' + ], + }, +) diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_grpc_message_bridge.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_grpc_message_bridge.sh new file mode 100755 index 00000000..af81784a --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_grpc_message_bridge.sh @@ -0,0 +1,17 @@ +#!/usr/bin/env bash +set -euo pipefail + +GRPC_HOST="${GRPC_HOST:-0.0.0.0}" +GRPC_PORT="${GRPC_PORT:-50051}" +BRIDGE_CONFIG_PATH="${BRIDGE_CONFIG_PATH:-$(dirname "$0")/config/bridge_config.yaml}" +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +# Set USE_LOCAL_GRPC_SERVER=1 to start a local gRPC server before the bridge node. +if [[ "${USE_LOCAL_GRPC_SERVER:-}" == "1" ]]; then + "$SCRIPT_DIR/start_local_grpc_server.sh" +fi + +RMW_IMPLEMENTATION=rmw_fastrtps_cpp ros2 launch grpc_message_bridge bridge.launch.py \ + grpc_host:="$GRPC_HOST" \ + grpc_port:="$GRPC_PORT" \ + config_path:="$BRIDGE_CONFIG_PATH" diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_local_grpc_server.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_local_grpc_server.sh new file mode 100755 index 00000000..1d26f607 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_local_grpc_server.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash +set -euo pipefail + +GRPC_HOST="${GRPC_HOST:-0.0.0.0}" +GRPC_PORT="${GRPC_PORT:-50051}" +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PIDFILE="${TMPDIR:-/tmp}/grpc_local_server.pid" +LOGFILE="${TMPDIR:-/tmp}/grpc_local_server.log" + +if [[ -f "$PIDFILE" ]] && kill -0 "$(cat "$PIDFILE")" 2>/dev/null; then + echo "Local gRPC server already running (pid $(cat "$PIDFILE"))" + exit 0 +fi + +python3 "$SCRIPT_DIR/grpc_server.py" --host "$GRPC_HOST" --port "$GRPC_PORT" \ + > "$LOGFILE" 2>&1 & +echo $! > "$PIDFILE" +echo "Local gRPC server started (pid $(cat "$PIDFILE")) on ${GRPC_HOST}:${GRPC_PORT}" +echo "Logs: $LOGFILE" diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/stop_local_grpc_server.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/stop_local_grpc_server.sh new file mode 100755 index 00000000..754656c1 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/stop_local_grpc_server.sh @@ -0,0 +1,20 @@ +#!/usr/bin/env bash +set -euo pipefail + +PIDFILE="${TMPDIR:-/tmp}/grpc_local_server.pid" + +if [[ ! -f "$PIDFILE" ]]; then + echo "No pidfile found at $PIDFILE -- server may not be running" + exit 0 +fi + +PID="$(cat "$PIDFILE")" + +if kill -0 "$PID" 2>/dev/null; then + kill "$PID" + echo "Local gRPC server stopped (pid $PID)" +else + echo "Process $PID not found -- already stopped" +fi + +rm -f "$PIDFILE" From c5191b7c794272c1c66a039d8f92a1091d63bb07 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Wed, 1 Jul 2026 11:52:58 +0200 Subject: [PATCH 04/18] added grpc message bridge and update mqtt message bridge Signed-off-by: Andrew Koerner --- .../grpc_message_bridge/.gitignore | 1 + .../grpc_message_bridge/.secrets.env.template | 4 + .../grpc_message_bridge/README.md | 106 ++++ .../grpc_message_bridge/docker-compose.yaml | 14 + .../grpc_message_bridge/auth.py | 118 +++++ .../grpc_message_bridge/bridge_node.py | 417 ++++++++-------- .../grpc_message_bridge/codec.py | 191 ++++++++ .../grpc_message_bridge/proto_loader.py | 170 +++++++ .../grpc_message_bridge/proto_registry.py | 89 ++++ .../grpc_message_bridge/servicer_factory.py | 99 ++++ .../grpc_message_bridge/proto/.gitkeep | 0 .../scripts/test_connection.py | 284 +++++++++++ .../scripts/test_frontend_client.py | 51 ++ .../scripts/test_integration.py | 463 ++++++++++++++++++ .../scripts/test_vehicle_client.py | 69 +++ .../tools/grps_oath_test.sh | 3 + .../scripts/bridge_mqtt_args.py | 83 ++++ .../scripts/mqtt_common.sh | 62 +-- .../mqtt_message_bridge/scripts/mqtt_log.sh | 48 ++ .../scripts/mqtt_log_writer.py | 31 ++ 20 files changed, 2050 insertions(+), 253 deletions(-) create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/.secrets.env.template create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/README.md create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-compose.yaml create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/auth.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/codec.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/proto_loader.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/proto_registry.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/servicer_factory.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/proto/.gitkeep create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_connection.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_frontend_client.py create mode 100755 ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_integration.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_vehicle_client.py create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grps_oath_test.sh create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/bridge_mqtt_args.py create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log.sh create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log_writer.py diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore new file mode 100644 index 00000000..9551e2ae --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore @@ -0,0 +1 @@ +.secrets.env diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.secrets.env.template b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.secrets.env.template new file mode 100644 index 00000000..3719d1b2 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.secrets.env.template @@ -0,0 +1,4 @@ +AUTH_ENDPOINT=https://some_oath_provider.com/auth/token +GRPC_ENDPOINT=some_grpc_service.com:443 +CLIENT_ID=some_client_id +CLIENT_SECRET=some_super_secret_secret diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/README.md b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/README.md new file mode 100644 index 00000000..0915669c --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/README.md @@ -0,0 +1,106 @@ +# grpc_message_bridge + +## Setup + +```bash +cp .secrets.env.template .secrets.env +# fill in CLIENT_SECRET +make build +``` + +## Run + +```bash +make start +make stop +make restart +make logs +``` + +## Inspect live data + +Subscribe and pretty-print incoming messages from the supervision backend. + +```bash +# All message types, 30s (default) +make inspect + +# Telemetry only, 60s +make inspect TYPE=TELEMETRY DURATION=60 + +# Notifications only, 120s +make inspect TYPE=NOTIFICATIONS DURATION=120 + +# All types, specific vehicles +make inspect TYPE=ALL VEHICLE="MV-001 MV-002" + +# Combine +make inspect TYPE=TELEMETRY DURATION=300 VEHICLE=MV-001 +``` + +`TYPE` options: `ALL` (default) · `TELEMETRY` · `NOTIFICATIONS` + +## Test + +```bash +# All 7 scenarios +make test + +# Single scenario +make test SCENARIO=4 +``` + +## ROS topics + +All messages are JSON strings on `std_msgs/msg/String`. + +### Incoming (supervision backend → ROS) + +| Topic | Content | +|---|---| +| `/supervision/telemetry` | `VehicleTelemetryUpdate` -- vehicle id, state, position, velocity, battery, obstacles | +| `/supervision/notifications` | `NotificationMessage` -- id, vehicle id, type, severity, title, message | +| `/supervision/ack/rx` | `Ack` -- success, error_msg | +| `/supervision/signaling/rx` | `SignalMessage` -- WebRTC signaling (offer/answer/ICE) | + +### Outgoing (ROS → supervision backend) + +| Topic | Content | +|---|---| +| `/supervision/subscription` | `SubscriptionRequest` -- type (TELEMETRY/NOTIFICATIONS/ALL), vehicle_ids | +| `/supervision/ack/tx` | `Ack` | +| `/supervision/signaling/tx` | `SignalMessage` | + +### Examples + +Subscribe to telemetry: +```bash +ros2 topic echo /supervision/telemetry +``` + +Subscribe to notifications: +```bash +ros2 topic echo /supervision/notifications +``` + +Send a subscription request (all vehicles, telemetry only): +```bash +ros2 topic pub --once /supervision/subscription std_msgs/msg/String \ + '{"data": "{\"type\": \"TELEMETRY\", \"vehicleIds\": []}"}' +``` + +Send a subscription request filtered to specific vehicles: +```bash +ros2 topic pub --once /supervision/subscription std_msgs/msg/String \ + '{"data": "{\"type\": \"ALL\", \"vehicleIds\": [\"MV-001\", \"MV-002\"]}"}' +``` + +List all active bridge topics: +```bash +ros2 topic list | grep supervision +``` + +Check message rate: +```bash +ros2 topic hz /supervision/telemetry +``` diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-compose.yaml b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-compose.yaml new file mode 100644 index 00000000..bac4d0ae --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-compose.yaml @@ -0,0 +1,14 @@ +services: + bridge: + build: . + image: grpc_message_bridge:latest + network_mode: host + env_file: + - .secrets.env + environment: + BRIDGE_CONFIG_PATH: /config/bridge_config.yaml + GRPC_HOST: "0.0.0.0" + GRPC_PORT: "50051" + volumes: + - ./config/bridge_config.yaml:/config/bridge_config.yaml:ro + restart: unless-stopped diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/auth.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/auth.py new file mode 100644 index 00000000..2ed022fb --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/auth.py @@ -0,0 +1,118 @@ +""" +TLS + OAuth2 client-credentials channel factory. + +Spec: Supervision External Gateway gTA Integration Guide Phase 1 + - Token endpoint: POST /auth/token + - Body: grant_type, client_id, client_secret only + - Token TTL: 1800s (30 minutes) + - gRPC auth: Authorization: Bearer + +Environment variables (via .secrets.env): + GRPC_ENDPOINT host:port e.g. supervision.dev-motor-ai.com:443 + AUTH_ENDPOINT full token URL e.g. https://supervision.dev-motor-ai.com/auth/token + CLIENT_ID OAuth2 client_id + CLIENT_SECRET OAuth2 client_secret +""" + +import json +import logging +import os +import threading +import time +import urllib.error +import urllib.parse +import urllib.request +from typing import Optional + +import grpc + +log = logging.getLogger(__name__) + +_TOKEN_REFRESH_BUFFER_S = 120 # refresh 2 min before 30-min TTL expires + + +class _OAuth2CallCredentials(grpc.AuthMetadataPlugin): + def __init__(self, auth_endpoint: str, client_id: str, client_secret: str): + self._endpoint = auth_endpoint + self._client_id = client_id + self._client_secret = client_secret + self._token: Optional[str] = None + self._expires_at: float = 0.0 + self._lock = threading.Lock() + + def _fetch(self) -> None: + body = urllib.parse.urlencode({ + 'grant_type': 'client_credentials', + 'client_id': self._client_id, + 'client_secret': self._client_secret, + }).encode() + req = urllib.request.Request( + self._endpoint, + data = body, + headers = {'Content-Type': 'application/x-www-form-urlencoded'}, + method = 'POST', + ) + try: + with urllib.request.urlopen(req, timeout=30) as resp: + data = json.loads(resp.read()) + except urllib.error.HTTPError as e: + body = e.read().decode(errors='replace') + raise RuntimeError(f'Token fetch HTTP {e.code}: {body}') from e + + if 'access_token' not in data: + raise RuntimeError(f'Token response missing access_token: {data}') + + ttl = data.get('expires_in', 1800) + self._token = data['access_token'] + self._expires_at = time.monotonic() + ttl - _TOKEN_REFRESH_BUFFER_S + log.info('OAuth2 token refreshed (expires_in=%ds)', ttl) + + def _ensure_valid_token(self) -> str: + if self._token and time.monotonic() < self._expires_at: + return self._token + last_err = None + for attempt in range(3): + try: + self._fetch() + return self._token + except Exception as e: + last_err = e + log.warning('Token fetch attempt %d/3 failed: %s', attempt + 1, e) + if attempt < 2: + time.sleep(2 ** attempt) + raise RuntimeError(f'Token fetch failed after 3 attempts: {last_err}') + + def __call__(self, context, callback): + with self._lock: + try: + token = self._ensure_valid_token() + except Exception as e: + log.error('Cannot obtain OAuth2 token: %s', e) + callback([], grpc.StatusCode.UNAUTHENTICATED) + return + callback([('authorization', f'Bearer {token}')], None) + + +def make_channel(address: Optional[str] = None) -> grpc.Channel: + """ + Return a TLS+OAuth2 gRPC channel. + Falls back to insecure for local dev when AUTH_ENDPOINT is unset. + """ + addr = address or os.environ.get('GRPC_ENDPOINT', '') + auth_endpoint = os.environ.get('AUTH_ENDPOINT', '') + client_id = os.environ.get('CLIENT_ID', '') + client_secret = os.environ.get('CLIENT_SECRET', '') + + if not addr: + raise ValueError('No gRPC address: pass address or set GRPC_ENDPOINT') + + if auth_endpoint and client_id and client_secret: + log.info('Creating TLS+OAuth2 channel to %s', addr) + plugin = _OAuth2CallCredentials(auth_endpoint, client_id, client_secret) + tls_creds = grpc.ssl_channel_credentials() + call_creds = grpc.metadata_call_credentials(plugin, name='oauth2') + creds = grpc.composite_channel_credentials(tls_creds, call_creds) + return grpc.secure_channel(addr, creds) + + log.warning('No auth credentials set -- using insecure channel to %s', addr) + return grpc.insecure_channel(addr) diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py index 058de201..3887104d 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py @@ -2,17 +2,27 @@ import queue import threading import time -import yaml + import grpc import rclpy +import yaml from concurrent import futures -from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor -from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy -from . import ros_bridge_pb2, ros_bridge_pb2_grpc -from .utils import load_msg_type, msg_to_bytes, bytes_to_msg, msg_to_json, json_to_msg, msg_to_cdr_json, cdr_json_to_msg - -_STR_TYPE = 'std_msgs/msg/String' +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy + +from .codec import ( + active_oneof_field, + load_ros_type, + make_ros_deserializer, + make_ros_serializer, + proto_field_set, + proto_field_to_bytes, + wire_ros_type, +) +from .proto_registry import FieldMapping, StreamDef, STREAMS +from .auth import make_channel +from .servicer_factory import build_servicer _DURABILITY = { 'volatile': DurabilityPolicy.VOLATILE, @@ -24,254 +34,231 @@ } -def _wire_type(ros_type: str, fmt: str) -> str: - return _STR_TYPE if fmt in ('json', 'cdr_json') else ros_type - - -def _serializer(ros_type: str, fmt: str): - if fmt == 'json': - return lambda msg, rt=ros_type: msg_to_json(msg, rt) - if fmt == 'cdr_json': - return lambda msg, rt=ros_type: msg_to_cdr_json(msg, rt) - return msg_to_bytes - - -def _deserializer(msg_type, fmt: str): - if fmt == 'json': - return lambda data, mt=msg_type: json_to_msg(data, mt) - if fmt == 'cdr_json': - return lambda data, mt=msg_type: cdr_json_to_msg(data, mt) - return lambda data, mt=msg_type: bytes_to_msg(data, mt) - - -def _qos_from_mapping(mapping: dict, default_reliability: str = 'reliable') -> QoSProfile: +def _qos(cfg: dict, default_reliability: str = 'reliable') -> QoSProfile: return QoSProfile( - depth=mapping.get('qos_depth', 1), - durability=_DURABILITY.get(mapping.get('qos_durability', 'volatile'), DurabilityPolicy.VOLATILE), - reliability=_RELIABILITY.get(mapping.get('qos_reliability', default_reliability), + depth=cfg.get('qos_depth', 10), + durability=_DURABILITY.get(cfg.get('qos_durability', 'volatile'), DurabilityPolicy.VOLATILE), + reliability=_RELIABILITY.get(cfg.get('qos_reliability', default_reliability), _RELIABILITY[default_reliability]), history=HistoryPolicy.KEEP_LAST, ) -class RosBridgeServicer(ros_bridge_pb2_grpc.RosBridgeServicer): - """gRPC servicer: handles inbound Publish streams and outbound Subscribe streams.""" - - def __init__(self, node: 'ROS2GrpcBridge'): - self._node = node - - def Publish(self, request_iterator, context): - """Receive a stream of RosMessages from a remote publisher and forward to ROS.""" - for grpc_msg in request_iterator: - if self._node._shutdown_event.is_set(): - break - topic = grpc_msg.topic - pub = self._node.ros_pubs.get(topic) - if pub is None: - self._node.get_logger().warn(f'Received publish for unconfigured topic: {topic}') - continue - deser = self._node._grpc_deserializers.get(topic) - if deser is None: - continue - try: - msg = deser(grpc_msg.payload) - self._node._grpc_to_ros_queue.put((pub, msg)) - except Exception as e: - self._node.get_logger().error(f'Deser failed on {topic}: {e}') - return ros_bridge_pb2.Ack(ok=True) - - def Subscribe(self, request, context): - """Stream RosMessages to a remote subscriber for a configured topic.""" - topic = request.topic - sub_queue = queue.Queue() - - with self._node._subscriber_lock: - self._node._grpc_subscribers.setdefault(topic, []).append(sub_queue) - - self._node.get_logger().info(f'gRPC subscriber connected: {topic}') - try: - while not self._node._shutdown_event.is_set() and context.is_active(): - try: - grpc_msg = sub_queue.get(timeout=0.1) - yield grpc_msg - except queue.Empty: - continue - finally: - with self._node._subscriber_lock: - subs = self._node._grpc_subscribers.get(topic, []) - if sub_queue in subs: - subs.remove(sub_queue) - self._node.get_logger().info(f'gRPC subscriber disconnected: {topic}') - - class ROS2GrpcBridge(Node): def __init__(self): super().__init__('grpc_bridge_node') self.declare_parameter('config_path', '') - self.declare_parameter('grpc_host', '0.0.0.0') - self.declare_parameter('grpc_port', 50051) - self.declare_parameter('grpc_server_address', '') + self.declare_parameter('grpc_host', '0.0.0.0') + self.declare_parameter('grpc_port', 50051) config_path = self.get_parameter('config_path').get_parameter_value().string_value if not config_path or not os.path.exists(config_path): - self.get_logger().error(f'Config file not found: {config_path}') + self.get_logger().fatal(f'Config not found: {config_path}') return - try: - with open(config_path, 'r') as f: - self.config = yaml.safe_load(f) - except Exception as e: - self.get_logger().error(f'Failed to load config: {e}') - return + with open(config_path) as f: + self.config = yaml.safe_load(f) - self.ros_subs = [] - self.ros_pubs = {} - self._grpc_deserializers = {} - self._grpc_to_ros_queue = queue.Queue() - self._grpc_subscribers = {} - self._subscriber_lock = threading.Lock() - self._shutdown_event = threading.Event() - self._grpc_clients = {} - self._grpc_channels = {} - self._grpc_pub_streams = {} + self.ros_pubs: dict = {} + self.ros_subs: list = [] + self.ros_publish_queue = queue.Queue() + self.stream_send_queues: dict = {} # stream.key -> Queue[proto_msg] + self.shutdown_event = threading.Event() + self._grpc_server = None + self._grpc_channels: dict = {} + + self._setup_publishers() self._setup_grpc_server() - self._setup_ros2_to_grpc() - self._setup_grpc_to_ros2() - self.create_timer(0.01, self._drain_grpc_to_ros_queue) + self._setup_server_streams() + self._setup_client_streams() + self.create_timer(0.01, self._drain_publish_queue) + + # ------------------------------------------------------------------ + # Publisher setup + # ------------------------------------------------------------------ + + def _setup_publishers(self): + """Pre-create ROS publishers for all recv_fields across enabled streams.""" + enabled = set(self.config.get('enabled_streams', list(STREAMS.keys()))) + + for key, stream in STREAMS.items(): + if key not in enabled: + continue + for fm in stream.recv_fields: + if fm.ros_topic in self.ros_pubs: + continue + wire_type = wire_ros_type(fm.ros_msg_type, fm.format) + ros_type = load_ros_type(wire_type) + self.ros_pubs[fm.ros_topic] = self.create_publisher( + ros_type, fm.ros_topic, _qos({}, 'best_effort')) + self.get_logger().info(f'Publisher: {fm.ros_topic}') + + # ------------------------------------------------------------------ + # gRPC server (bridge acts as server -- remote connects to us) + # ------------------------------------------------------------------ def _setup_grpc_server(self): host = self.get_parameter('grpc_host').get_parameter_value().string_value port = self.get_parameter('grpc_port').get_parameter_value().integer_value + self._grpc_server = grpc.server(futures.ThreadPoolExecutor(max_workers=10)) - ros_bridge_pb2_grpc.add_RosBridgeServicer_to_server(RosBridgeServicer(self), self._grpc_server) - listen_addr = f'{host}:{port}' - self._grpc_server.add_insecure_port(listen_addr) + + server_streams = self.config.get('server_streams', []) + for entry in server_streams: + key = entry['stream'] + stream = STREAMS.get(key) + if stream is None: + self.get_logger().error(f'Unknown stream: {key}') + continue + servicer = build_servicer(stream, self) + stream.add_servicer_fn(servicer, self._grpc_server) + self._setup_send_subscriptions(stream) + self.get_logger().info(f'Server stream registered: {key}') + + addr = f'{host}:{port}' + self._grpc_server.add_insecure_port(addr) self._grpc_server.start() - self.get_logger().info(f'gRPC server listening on {listen_addr}') + self.get_logger().info(f'gRPC server listening on {addr}') - def _get_client(self, address: str): - if address not in self._grpc_channels: - channel = grpc.insecure_channel(address) - self._grpc_channels[address] = channel - self._grpc_clients[address] = ros_bridge_pb2_grpc.RosBridgeStub(channel) - return self._grpc_clients[address] - - def _setup_ros2_to_grpc(self): - """Subscribe to ROS topics and stream messages to remote gRPC servers.""" - for mapping in self.config.get('ros2_to_grpc', []): - ros_topic = mapping['ros_topic'] - ros_type = mapping.get('msg_type', _STR_TYPE) - remote_addr = mapping.get('grpc_address', '') - fmt = mapping.get('format', 'cdr') - qos = _qos_from_mapping(mapping, default_reliability='reliable') - msg_type = load_msg_type(ros_type) - serialize = _serializer(ros_type, fmt) - seq = [0] + # ------------------------------------------------------------------ + # Client streams (bridge dials out to a remote gRPC server) + # ------------------------------------------------------------------ + def _setup_client_streams(self): + for entry in self.config.get('client_streams', []): + key = entry['stream'] + remote_addr = entry.get('grpc_address') or os.environ.get('GRPC_ENDPOINT', '') if not remote_addr: - self.get_logger().error(f'ros2_to_grpc entry for {ros_topic} missing grpc_address') + self.get_logger().error(f'client_stream {key}: no grpc_address and GRPC_ENDPOINT not set') + continue + stream = STREAMS.get(key) + if stream is None: + self.get_logger().error(f'Unknown stream: {key}') continue - # Stream queue feeding a persistent per-topic publisher thread. - stream_queue: queue.Queue = queue.Queue() - self._grpc_pub_streams[ros_topic] = stream_queue + self._setup_send_subscriptions(stream) + send_queue = self.stream_send_queues.setdefault(stream.key, queue.Queue()) - def _stream_worker(addr=remote_addr, topic=ros_topic, rtype=ros_type, f=fmt, sq=stream_queue): - while not self._shutdown_event.is_set(): + def _sender(sq=send_queue, shutdown=self.shutdown_event): + """Yield queued messages, keeping the stream open until shutdown.""" + while not shutdown.is_set(): try: - stub = self._get_client(addr) - stub.Publish(iter(sq.get, None)) - except grpc.RpcError as e: - self.get_logger().warn(f'gRPC publish error on {topic}: {e.details()} -- retrying in 2s') - time.sleep(2) - - t = threading.Thread(target=_stream_worker, daemon=True) - t.start() - - self.get_logger().info(f'R2G: {ros_topic} -> {remote_addr} [{fmt}]') - - def cb(msg, sq=stream_queue, topic=ros_topic, rtype=ros_type, f=fmt, s=seq, ser=serialize): - grpc_msg = ros_bridge_pb2.RosMessage( - topic = topic, - ros_type = rtype, - format = f, - payload = ser(msg), - seq = s[0], - stamp_ns = time.time_ns(), - ) - s[0] += 1 - sq.put(grpc_msg) - self.get_logger().debug(f'R2G queued: {topic}') - - self.ros_subs.append(self.create_subscription(msg_type, ros_topic, cb, qos)) - - def _setup_grpc_to_ros2(self): - """For each grpc_to_ros2 entry, register a ROS publisher and accept inbound gRPC Publish calls.""" - for mapping in self.config.get('grpc_to_ros2', []): - ros_topic = mapping['ros_topic'] - ros_type = mapping.get('msg_type', _STR_TYPE) - fmt = mapping.get('format', 'cdr') - wtype = _wire_type(ros_type, fmt) - qos = _qos_from_mapping(mapping, default_reliability='best_effort') - pub_type = load_msg_type(wtype) - m_type = load_msg_type(ros_type) - - self.ros_pubs[ros_topic] = self.create_publisher(pub_type, ros_topic, qos) - self._grpc_deserializers[ros_topic] = _deserializer(m_type, fmt) - self.get_logger().info(f'G2R: registered inbound topic {ros_topic} [{fmt}]') - - def _setup_grpc_subscribe_forwarders(self): - """ - For ros2_to_grpc entries that also want a remote Subscribe channel, - connect as a gRPC client subscriber and publish received messages locally. - Used when the remote side is a server and this node is the consumer. - """ - for mapping in self.config.get('grpc_subscribe_from', []): - ros_topic = mapping['ros_topic'] - ros_type = mapping.get('msg_type', _STR_TYPE) - remote_addr = mapping['grpc_address'] - fmt = mapping.get('format', 'cdr') - qos = _qos_from_mapping(mapping, default_reliability='best_effort') - wtype = _wire_type(ros_type, fmt) - pub_type = load_msg_type(wtype) - m_type = load_msg_type(ros_type) - deserialize = _deserializer(m_type, fmt) - - pub = self.create_publisher(pub_type, ros_topic, qos) - self.ros_pubs[ros_topic] = pub - self.get_logger().info(f'GSF: {remote_addr} -> {ros_topic} [{fmt}]') - - def _sub_worker(addr=remote_addr, topic=ros_topic, rtype=ros_type, f=fmt, p=pub, deser=deserialize): - while not self._shutdown_event.is_set(): + msg = sq.get(timeout=0.5) + if msg is None: + return + yield msg + except queue.Empty: + continue + + def _worker(addr=remote_addr, s=stream, sq=send_queue, sender=_sender): + while not self.shutdown_event.is_set(): try: - stub = self._get_client(addr) - req = ros_bridge_pb2.SubscribeRequest(topic=topic, ros_type=rtype, format=f) - for grpc_msg in stub.Subscribe(req): - if self._shutdown_event.is_set(): - break - try: - msg = deser(grpc_msg.payload) - self._grpc_to_ros_queue.put((p, msg)) - except Exception as e: - self.get_logger().error(f'Deser failed on {topic}: {e}') + channel = self._channel(addr) + stub = s.stub_cls(channel) + rpc = getattr(stub, s.rpc) + + if s.stream_type == 'bidi': + for recv_msg in rpc(sender()): + self._dispatch_recv(s, recv_msg) + + elif s.stream_type == 'server_streaming': + req = s.send_msg_cls() + for recv_msg in rpc(req): + self._dispatch_recv(s, recv_msg) + + elif s.stream_type == 'client_streaming': + rpc(sender()) + except grpc.RpcError as e: - self.get_logger().warn(f'gRPC subscribe error on {topic}: {e.details()} -- retrying in 2s') + self.get_logger().warn( + f'[{s.key}] {e.code().name}: {e.details()} -- reconnecting in 2s') time.sleep(2) - threading.Thread(target=_sub_worker, daemon=True).start() + threading.Thread(target=_worker, daemon=True).start() + self.get_logger().info(f'Client stream: {key} -> {remote_addr}') + + # ------------------------------------------------------------------ + # Server streams (identical to client but called from _setup_grpc_server) + # ------------------------------------------------------------------ + + def _setup_server_streams(self): + """Server-mode streams also need their send-direction ROS subscriptions.""" + # Already handled inside _setup_grpc_server per entry. + pass + + # ------------------------------------------------------------------ + # Shared: subscribe to ROS topics that feed into a stream's send direction + # ------------------------------------------------------------------ + + def _setup_send_subscriptions(self, stream: StreamDef): + for fm in stream.send_fields: + if any(s.topic_name == fm.ros_topic for s in self.ros_subs): + continue # already subscribed + + ros_type = load_ros_type(fm.ros_msg_type) + serialize = make_ros_serializer(fm.ros_msg_type, fm.format) + send_queue = self.stream_send_queues.setdefault(stream.key, queue.Queue()) + + def cb(ros_msg, f=fm, cls=stream.send_msg_cls, sq=send_queue, ser=serialize): + try: + payload = ser(ros_msg) + proto_msg = proto_field_set(cls, f.field_name, payload, f.format) + sq.put(proto_msg) + except Exception as e: + self.get_logger().error( + f'[{stream.key}] pack {f.field_name}: {e}') + + sub = self.create_subscription(ros_type, fm.ros_topic, cb, _qos({}, 'reliable')) + self.ros_subs.append(sub) + self.get_logger().info(f'Send sub: {fm.ros_topic} -> {stream.key}.{fm.field_name}') + + # ------------------------------------------------------------------ + # Receive dispatch (used by client-mode streams) + # ------------------------------------------------------------------ + + def _dispatch_recv(self, stream: StreamDef, proto_msg): + field_name = active_oneof_field(proto_msg) + fm = stream.recv_field_map.get(field_name) + if fm is None: + return + + payload = proto_field_to_bytes(proto_msg, field_name, fm.format) + if payload is None: + return + + wire_type = wire_ros_type(fm.ros_msg_type, fm.format) + ros_type = load_ros_type(wire_type) + deserialize = make_ros_deserializer(ros_type, fm.format) + + try: + ros_msg = deserialize(payload) + except Exception as e: + self.get_logger().error(f'[{stream.key}] deser {field_name}: {e}') + return + + pub = self.ros_pubs.get(fm.ros_topic) + if pub: + self.ros_publish_queue.put((pub, ros_msg)) + + # ------------------------------------------------------------------ + + def _channel(self, address: str) -> grpc.Channel: + if address not in self._grpc_channels: + self._grpc_channels[address] = make_channel(address) + return self._grpc_channels[address] - def _drain_grpc_to_ros_queue(self): - while not self._grpc_to_ros_queue.empty(): - pub, msg = self._grpc_to_ros_queue.get_nowait() + def _drain_publish_queue(self): + while not self.ros_publish_queue.empty(): + pub, msg = self.ros_publish_queue.get_nowait() pub.publish(msg) def shutdown(self): - self._shutdown_event.set() - # Sentinel None unblocks iter(queue.get, None) in publisher threads. - for sq in self._grpc_pub_streams.values(): + self.shutdown_event.set() + for sq in self.stream_send_queues.values(): sq.put(None) - self._grpc_server.stop(grace=2) + if self._grpc_server: + self._grpc_server.stop(grace=2) for ch in self._grpc_channels.values(): ch.close() diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/codec.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/codec.py new file mode 100644 index 00000000..3bf30d11 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/codec.py @@ -0,0 +1,191 @@ +""" +Codec: translates between proto messages and ROS messages. + +All logic for packing/unpacking proto oneofs lives here so bridge_node.py +stays free of serialization concerns. +""" + +import json +import sys +from typing import Any, Callable, Optional + +from rclpy.serialization import serialize_message, deserialize_message +from rosidl_runtime_py import message_to_ordereddict, set_message_fields + +_STR_TYPE = 'std_msgs/msg/String' + + +# --------------------------------------------------------------------------- +# ROS type loading +# --------------------------------------------------------------------------- + +def load_ros_type(msg_type_str: str): + pkg, interface, name = msg_type_str.split('/') + import importlib + return getattr(importlib.import_module(f'{pkg}.{interface}'), name) + + +def wire_ros_type(ros_type: str, fmt: str) -> str: + """The actual ROS type that travels on the wire (may differ from logical type for json formats).""" + return _STR_TYPE if fmt in ('json', 'cdr_json') else ros_type + + +# --------------------------------------------------------------------------- +# ROS serializers / deserializers +# --------------------------------------------------------------------------- + +def make_ros_serializer(ros_type: str, fmt: str) -> Callable: + if fmt == 'json': + def _to_json(msg, rt=ros_type) -> bytes: + obj = message_to_ordereddict(msg) + obj['datatype'] = rt + return json.dumps(obj).encode() + return _to_json + + if fmt == 'cdr_json': + def _to_cdr_json(msg, rt=ros_type) -> bytes: + from std_msgs.msg import String + obj = message_to_ordereddict(msg) + obj['datatype'] = rt + wrapper = String(data=json.dumps(obj)) + return serialize_message(wrapper) + return _to_cdr_json + + return serialize_message + + +def make_ros_deserializer(msg_type, fmt: str) -> Callable: + if fmt == 'json': + def _from_json(data: bytes, mt=msg_type): + obj = json.loads(data.decode()) + obj.pop('datatype', None) + obj.pop('topic', None) + msg = mt() + set_message_fields(msg, obj) + return msg + return _from_json + + if fmt == 'cdr_json': + def _from_cdr_json(data: bytes, mt=msg_type): + from std_msgs.msg import String + wrapper = deserialize_message(data, String) + obj = json.loads(wrapper.data) + obj.pop('datatype', None) + obj.pop('topic', None) + msg = mt() + set_message_fields(msg, obj) + return msg + return _from_cdr_json + + return lambda data, mt=msg_type: deserialize_message(data, mt) + + +# --------------------------------------------------------------------------- +# Proto <-> bytes +# --------------------------------------------------------------------------- + +def proto_field_to_bytes(proto_msg: Any, field_name: str, fmt: str) -> Optional[bytes]: + """ + Extract a field from a proto message and return it as bytes suitable + for feeding to a ROS deserializer. + + For json/cdr_json formats the field value is serialized to JSON bytes. + For cdr the field value is serialized to proto bytes (caller's ROS + deserializer must accept them). + """ + field_desc = type(proto_msg).DESCRIPTOR.fields_by_name.get(field_name) + if field_desc is None: + return None + + value = getattr(proto_msg, field_name, None) + if value is None: + return None + + # Scalar bytes/string -- return directly. + if field_desc.type in (field_desc.TYPE_BYTES, field_desc.TYPE_STRING): + return value if isinstance(value, bytes) else value.encode() + + if fmt in ('json', 'cdr_json'): + # Serialize proto message to dict then to JSON bytes. + d = _proto_to_dict(value) + return json.dumps(d).encode() + + # cdr: serialize proto message to its binary wire format. + return value.SerializeToString() + + +def proto_field_set(proto_msg_cls, field_name: str, payload: bytes, fmt: str) -> Any: + """ + Create a new proto message of proto_msg_cls with field_name set from payload bytes. + payload is what came out of a ROS serializer. + """ + instance = proto_msg_cls() + field_desc = proto_msg_cls.DESCRIPTOR.fields_by_name.get(field_name) + if field_desc is None: + raise ValueError(f'Field {field_name!r} not found on {proto_msg_cls.DESCRIPTOR.name}') + + # Scalar bytes/string -- set directly. + if field_desc.type in (field_desc.TYPE_BYTES, field_desc.TYPE_STRING): + setattr(instance, field_name, payload) + return instance + + # Nested message -- find its class and populate. + nested_cls = _find_proto_class(field_desc.message_type.full_name) + if nested_cls is None: + raise LookupError(f'Cannot resolve proto class: {field_desc.message_type.full_name}') + + if fmt in ('json', 'cdr_json'): + d = json.loads(payload.decode()) + _dict_to_proto(nested_cls, d, getattr(instance, field_name)) + else: + getattr(instance, field_name).MergeFrom(nested_cls.FromString(payload)) + + return instance + + +# --------------------------------------------------------------------------- +# oneof dispatch +# --------------------------------------------------------------------------- + +def active_oneof_field(proto_msg: Any) -> str: + """Return the name of the set oneof branch, or '' if none.""" + for oneof in type(proto_msg).DESCRIPTOR.oneofs: + f = proto_msg.WhichOneof(oneof.name) + if f: + return f + return '' + + +# --------------------------------------------------------------------------- +# Internal helpers +# --------------------------------------------------------------------------- + +def _proto_to_dict(msg) -> dict: + """Shallow proto-message-to-dict, suitable for JSON serialization.""" + from google.protobuf.json_format import MessageToDict + return MessageToDict(msg, preserving_proto_field_name=True, including_default_value_fields=False) + + +def _dict_to_proto(proto_cls, d: dict, target=None): + from google.protobuf.json_format import ParseDict + return ParseDict(d, target if target is not None else proto_cls()) + + +def _find_proto_class(full_name: str) -> Optional[type]: + from google.protobuf import symbol_database + try: + return symbol_database.Default().GetSymbol(full_name) + except KeyError: + pass + # Fall back to scanning loaded _pb2 modules. + for mod in list(sys.modules.values()): + if not getattr(mod, '__name__', '').endswith('_pb2'): + continue + for attr in dir(mod): + obj = getattr(mod, attr, None) + try: + if obj and hasattr(obj, 'DESCRIPTOR') and obj.DESCRIPTOR.full_name == full_name: + return obj + except Exception: + pass + return None diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/proto_loader.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/proto_loader.py new file mode 100644 index 00000000..6be02bc4 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/proto_loader.py @@ -0,0 +1,170 @@ +import importlib.util +import logging +import os +import subprocess +import sys +from dataclasses import dataclass +from typing import Any, Callable, Dict, Optional + +import yaml + +log = logging.getLogger(__name__) + +_CUSTOM_PROTOS_DIR = os.path.join(os.path.dirname(__file__), '..', 'custom_protos') +_REGISTRY_PATH = os.path.join(_CUSTOM_PROTOS_DIR, 'registry.yaml') + + +@dataclass +class CustomServiceSpec: + proto_module: str + service_name: str + stub_class: str + servicer_base: str + publish_rpc: str + subscribe_rpc: str + message_class: str + ack_class: str + + pb2_mod: Any = None + pb2_grpc_mod: Any = None + + @property + def stub(self): + return getattr(self.pb2_grpc_mod, self.stub_class) + + @property + def servicer(self): + return getattr(self.pb2_grpc_mod, self.servicer_base) + + @property + def add_servicer_fn(self) -> Callable: + fn_name = f'add_{self.servicer_base}_to_server' + return getattr(self.pb2_grpc_mod, fn_name) + + @property + def message_cls(self): + return getattr(self.pb2_mod, self.message_class) + + @property + def ack_cls(self): + return getattr(self.pb2_mod, self.ack_class) + + +_registry: Dict[str, CustomServiceSpec] = {} + + +def _compile_proto(proto_path: str, out_dir: str) -> bool: + result = subprocess.run( + [ + sys.executable, '-m', 'grpc_tools.protoc', + f'-I{os.path.dirname(proto_path)}', + f'--python_out={out_dir}', + f'--grpc_python_out={out_dir}', + proto_path, + ], + capture_output=True, + text=True, + ) + if result.returncode != 0: + log.error('protoc failed for %s:\n%s', proto_path, result.stderr) + return False + return True + + +def _fix_grpc_imports(grpc_py_path: str) -> None: + """Rewrite absolute pb2 imports to relative so modules load from custom_protos/.""" + with open(grpc_py_path, 'r') as f: + src = f.read() + lines = [] + for line in src.splitlines(): + if line.startswith('import ') and line.endswith('_pb2'): + mod = line.split()[1] + line = f'from . import {mod}' + lines.append(line) + with open(grpc_py_path, 'w') as f: + f.write('\n'.join(lines)) + + +def _load_module(name: str, path: str): + spec = importlib.util.spec_from_file_location(name, path) + module = importlib.util.module_from_spec(spec) + sys.modules[name] = module + spec.loader.exec_module(module) + return module + + +def load_custom_protos(custom_protos_dir: Optional[str] = None) -> Dict[str, CustomServiceSpec]: + global _registry + protos_dir = os.path.abspath(custom_protos_dir or _CUSTOM_PROTOS_DIR) + registry_path = os.path.join(protos_dir, 'registry.yaml') + + if not os.path.isdir(protos_dir): + log.debug('custom_protos directory not found, skipping: %s', protos_dir) + return _registry + + if not os.path.isfile(registry_path): + log.debug('no registry.yaml in %s, skipping custom protos', protos_dir) + return _registry + + with open(registry_path) as f: + registry_data = yaml.safe_load(f) or {} + + services = registry_data.get('services') or [] + if not services: + log.debug('registry.yaml has no services entries') + return _registry + + # Ensure the protos dir is importable so relative imports in generated code work. + init_path = os.path.join(protos_dir, '__init__.py') + if not os.path.exists(init_path): + open(init_path, 'w').close() + + if protos_dir not in sys.path: + sys.path.insert(0, os.path.dirname(protos_dir)) + + for entry in services: + mod_name = entry['proto_module'] # e.g. example_sensor_pb2 + base_name = mod_name.replace('_pb2', '') # e.g. example_sensor + pb2_path = os.path.join(protos_dir, f'{mod_name}.py') + pb2_grpc_path = os.path.join(protos_dir, f'{base_name}_pb2_grpc.py') + + if not os.path.exists(pb2_path): + proto_file = os.path.join(protos_dir, f'{base_name}.proto') + if not os.path.exists(proto_file): + log.error('proto file not found: %s', proto_file) + continue + log.info('compiling %s', proto_file) + if not _compile_proto(proto_file, protos_dir): + continue + if os.path.exists(pb2_grpc_path): + _fix_grpc_imports(pb2_grpc_path) + + try: + pkg_prefix = f'custom_protos.{mod_name}' + pb2_mod = _load_module(pkg_prefix, pb2_path) + grpc_prefix = f'custom_protos.{base_name}_pb2_grpc' + pb2_grpc_mod = _load_module(grpc_prefix, pb2_grpc_path) + except Exception as e: + log.error('failed to load %s: %s', mod_name, e) + continue + + spec = CustomServiceSpec( + proto_module = mod_name, + service_name = entry['service_name'], + stub_class = entry['stub_class'], + servicer_base = entry['servicer_base'], + publish_rpc = entry['publish_rpc'], + subscribe_rpc = entry['subscribe_rpc'], + message_class = entry['message_class'], + ack_class = entry['ack_class'], + pb2_mod = pb2_mod, + pb2_grpc_mod = pb2_grpc_mod, + ) + _registry[entry['service_name']] = spec + log.info('registered custom gRPC service: %s', entry['service_name']) + + return _registry + + +def get_service(service_name: str) -> Optional[CustomServiceSpec]: + return _registry.get(service_name) diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/proto_registry.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/proto_registry.py new file mode 100644 index 00000000..bd5c3688 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/proto_registry.py @@ -0,0 +1,89 @@ +""" +Typed registry of gRPC streams and their ROS topic mappings. + +The bridge is a CLIENT that dials supervision.dev-motor-ai.com:443. + +ClientStream directions: + send (ClientMessage) -- bridge -> server: subscription requests, acks, signaling + recv (ServerMessage) -- server -> bridge: telemetry updates, notifications, acks + +Adding a new stream: + 1. Add .proto files under proto/ and run `make gen_proto`. + 2. Define a StreamDef below and add it to STREAMS. +""" + +from dataclasses import dataclass, field +from typing import Any, Dict, List, Type + + +from client import stream_pb2 as client_stream_pb2 +from client import stream_pb2_grpc as client_stream_grpc + + +@dataclass +class FieldMapping: + field_name: str + ros_topic: str + ros_msg_type: str = 'std_msgs/msg/String' + format: str = 'json' + + +@dataclass +class StreamDef: + service_name: str + rpc: str + stream_type: str # bidi | client_streaming | server_streaming + send_msg_cls: Type[Any] # proto class the bridge sends to the server + recv_msg_cls: Type[Any] # proto class the bridge receives from the server + stub_cls: Type[Any] + add_servicer_fn: Any + servicer_base: Type[Any] + send_fields: List[FieldMapping] = field(default_factory=list) + recv_fields: List[FieldMapping] = field(default_factory=list) + + @property + def key(self) -> str: + return f'{self.service_name}.{self.rpc}' + + @property + def recv_field_map(self) -> Dict[str, FieldMapping]: + return {f.field_name: f for f in self.recv_fields} + + @property + def send_field_map(self) -> Dict[str, FieldMapping]: + return {f.field_name: f for f in self.send_fields} + + +CLIENT_STREAM = StreamDef( + service_name = 'client.ClientService', + rpc = 'ClientStream', + stream_type = 'bidi', + + # Bridge sends ClientMessage to server + send_msg_cls = client_stream_pb2.ClientMessage, + # Bridge receives ServerMessage from server + recv_msg_cls = client_stream_pb2.ServerMessage, + + stub_cls = client_stream_grpc.ClientServiceStub, + add_servicer_fn = client_stream_grpc.add_ClientServiceServicer_to_server, + servicer_base = client_stream_grpc.ClientServiceServicer, + + # What we send to the server (ClientMessage oneofs) + send_fields=[ + FieldMapping('subscription', '/supervision/subscription', 'std_msgs/msg/String', 'json'), + FieldMapping('ack', '/supervision/ack/tx', 'std_msgs/msg/String', 'json'), + FieldMapping('signal_message','/supervision/signaling/tx', 'std_msgs/msg/String', 'json'), + ], + + # What we receive from the server (ServerMessage oneofs) -> ROS topics + recv_fields=[ + FieldMapping('telemetry', '/supervision/telemetry', 'std_msgs/msg/String', 'json'), + FieldMapping('notification', '/supervision/notifications', 'std_msgs/msg/String', 'json'), + FieldMapping('ack', '/supervision/ack/rx', 'std_msgs/msg/String', 'json'), + FieldMapping('signal_message','/supervision/signaling/rx', 'std_msgs/msg/String', 'json'), + ], +) + +STREAMS: Dict[str, StreamDef] = { + CLIENT_STREAM.key: CLIENT_STREAM, +} diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/servicer_factory.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/servicer_factory.py new file mode 100644 index 00000000..7b01d907 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/servicer_factory.py @@ -0,0 +1,99 @@ +""" +Builds gRPC servicer classes at runtime from StreamDef descriptors. + +Each servicer handles one RPC method. On every incoming proto message it: + 1. Identifies the active oneof field. + 2. Looks it up in the StreamDef's recv_field_map. + 3. Deserializes the field value to a ROS message and puts it on the ROS publish queue. + 4. For bidi/server-streaming: yields any queued outbound proto messages. +""" + +import queue +import threading +from typing import TYPE_CHECKING + +from .codec import ( + active_oneof_field, + load_ros_type, + make_ros_deserializer, + proto_field_to_bytes, + wire_ros_type, +) +from .proto_registry import FieldMapping, StreamDef + +if TYPE_CHECKING: + from .bridge_node import ROS2GrpcBridge + + +def build_servicer(stream: StreamDef, node: 'ROS2GrpcBridge'): + """ + Return a fully-wired servicer instance for stream.rpc. + + The servicer class is created dynamically so we can close over node and + stream without requiring a fixed class hierarchy. + """ + send_queue = node.stream_send_queues.setdefault(stream.key, queue.Queue()) + + def _handle_recv(proto_msg): + field_name = active_oneof_field(proto_msg) + mapping = stream.recv_field_map.get(field_name) + if mapping is None: + return + + payload = proto_field_to_bytes(proto_msg, field_name, mapping.format) + if payload is None: + return + + wire_type = wire_ros_type(mapping.ros_msg_type, mapping.format) + ros_type = load_ros_type(wire_type) + deserialize = make_ros_deserializer(ros_type, mapping.format) + + try: + ros_msg = deserialize(payload) + except Exception as e: + node.get_logger().error(f'[{stream.key}] deserialize {field_name}: {e}') + return + + pub = node.ros_pubs.get(mapping.ros_topic) + if pub: + node.ros_publish_queue.put((pub, ros_msg)) + else: + node.get_logger().warn(f'[{stream.key}] no publisher for {mapping.ros_topic}') + + rpc_name = stream.rpc + + if stream.stream_type == 'bidi': + def _bidi_rpc(self_svc, request_iterator, context): + for proto_msg in request_iterator: + if node.shutdown_event.is_set(): + break + _handle_recv(proto_msg) + # Drain any queued outbound messages without blocking. + while True: + try: + yield send_queue.get_nowait() + except queue.Empty: + break + methods = {rpc_name: _bidi_rpc} + + elif stream.stream_type == 'client_streaming': + def _client_stream_rpc(self_svc, request_iterator, context): + for proto_msg in request_iterator: + if node.shutdown_event.is_set(): + break + _handle_recv(proto_msg) + return stream.send_msg_cls() + methods = {rpc_name: _client_stream_rpc} + + else: # server_streaming + def _server_stream_rpc(self_svc, request, context): + _handle_recv(request) + while not node.shutdown_event.is_set() and context.is_active(): + try: + yield send_queue.get(timeout=0.1) + except queue.Empty: + continue + methods = {rpc_name: _server_stream_rpc} + + servicer_cls = type(f'{rpc_name}Servicer', (stream.servicer_base,), methods) + return servicer_cls() diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/proto/.gitkeep b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/proto/.gitkeep new file mode 100644 index 00000000..e69de29b diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_connection.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_connection.py new file mode 100644 index 00000000..cb430eda --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_connection.py @@ -0,0 +1,284 @@ +#!/usr/bin/env python3 +""" +Standalone connection test -- no ROS required. + +Tests in order: + 1. OAuth2 token fetch + 2. TLS + auth gRPC channel establishment + 3. VehicleStream handshake (sends one heartbeat, waits for any response) + 4. ClientStream handshake (sends subscription request, waits for any response) + +Usage: + # From the project root: + PYTHONPATH=proto/generated python3 scripts/test_connection.py + + # Or with explicit secrets: + AUTH_ENDPOINT=https://... CLIENT_ID=... CLIENT_SECRET=... TENANT_ID=dlr \ + FLEET_IDS=fleet_100,fleet_101 GRPC_ENDPOINT=host:443 \ + PYTHONPATH=proto/generated python3 scripts/test_connection.py + + # Load from .secrets.env: + set -a && source .secrets.env && set +a + PYTHONPATH=proto/generated python3 scripts/test_connection.py +""" + +import os +import sys +import time +import json +import threading +import urllib.request +import urllib.parse +import urllib.error + +# --------------------------------------------------------------------------- +# Colour helpers +# --------------------------------------------------------------------------- +_GREEN = '\033[92m' +_RED = '\033[91m' +_YELLOW = '\033[93m' +_RESET = '\033[0m' + +def ok(msg): print(f' {_GREEN}✓{_RESET} {msg}') +def fail(msg): print(f' {_RED}✗{_RESET} {msg}'); sys.exit(1) +def warn(msg): print(f' {_YELLOW}!{_RESET} {msg}') +def section(msg): print(f'\n{msg}') + + +# --------------------------------------------------------------------------- +# 1. Token fetch +# --------------------------------------------------------------------------- + +def test_token_fetch(auth_endpoint, client_id, client_secret, tenant_id, fleet_ids): + section('1. OAuth2 token fetch') + + params = { + 'grant_type': 'client_credentials', + 'client_id': client_id, + 'client_secret': client_secret, + } + if tenant_id: + params['tenant_id'] = tenant_id + if fleet_ids: + params['fleet_ids'] = ','.join(fleet_ids) + + print(f' POST {auth_endpoint}') + print(f' client_id={client_id} tenant_id={tenant_id} fleet_ids={fleet_ids}') + + body = urllib.parse.urlencode(params).encode() + req = urllib.request.Request( + auth_endpoint, + data = body, + headers = {'Content-Type': 'application/x-www-form-urlencoded'}, + method = 'POST', + ) + try: + with urllib.request.urlopen(req, timeout=30) as resp: + data = json.loads(resp.read()) + except urllib.error.HTTPError as e: + body = e.read().decode(errors='replace') + fail(f'HTTP {e.code}: {body}') + except Exception as e: + fail(str(e)) + + if 'access_token' not in data: + fail(f'No access_token in response: {data}') + + token = data['access_token'] + expires = data.get('expires_in', '?') + ok(f'Token received expires_in={expires}s token={token[:20]}...') + + # Decode JWT claims without verifying signature (for inspection only). + try: + import base64 + parts = token.split('.') + padding = 4 - len(parts[1]) % 4 + payload = base64.urlsafe_b64decode(parts[1] + '=' * padding) + claims = json.loads(payload) + ok(f'JWT claims: {json.dumps({k: claims[k] for k in claims if k in ("sub","aud","exp","tenant_id","fleet_ids","scope")}, default=str)}') + except Exception: + warn('Could not decode JWT payload (not a JWT or unexpected format)') + + return token + + +# --------------------------------------------------------------------------- +# 2. gRPC channel +# --------------------------------------------------------------------------- + +def test_channel(grpc_endpoint, token, tenant_id, fleet_ids): + section('2. gRPC channel (TLS)') + import grpc + + print(f' {grpc_endpoint}') + + metadata = [ + ('authorization', f'Bearer {token}'), + ] + if tenant_id: + metadata.append(('x-tenant-id', tenant_id)) + if fleet_ids: + metadata.append(('x-fleet-ids', ','.join(fleet_ids))) + + tls_creds = grpc.ssl_channel_credentials() + channel = grpc.secure_channel(grpc_endpoint, tls_creds) + + try: + grpc.channel_ready_future(channel).result(timeout=5) + ok(f'Channel ready') + except grpc.FutureTimeoutError: + fail('Channel did not become ready within 5s -- check GRPC_ENDPOINT and network') + + return channel, metadata + + +# --------------------------------------------------------------------------- +# 3. VehicleStream +# --------------------------------------------------------------------------- + +def test_vehicle_stream(channel, metadata): + section('3. VehicleStream (vehicle.VehicleService)') + import grpc + from vehicle import stream_pb2, stream_pb2_grpc + from messages import events_pb2, common_pb2 + + stub = stream_pb2_grpc.VehicleServiceStub(channel) + + def _messages(): + yield stream_pb2.VehicleMessage( + heartbeat = events_pb2.Heartbeat(status='test'), + metadata = common_pb2.MessageMetadata( + message_id = 'test-001', + vehicle_id = 'test-vehicle', + ), + ) + # Keep stream open briefly so the server has time to respond. + time.sleep(2) + + received = [] + error = [None] + + def _call(): + try: + for msg in stub.VehicleStream(_messages(), metadata=metadata, timeout=5): + received.append(msg) + except grpc.RpcError as e: + error[0] = e + + t = threading.Thread(target=_call) + t.start() + t.join(timeout=6) + + if error[0]: + code = error[0].code() + if code == grpc.StatusCode.UNIMPLEMENTED: + warn('UNIMPLEMENTED -- service exists but VehicleStream not active on this endpoint') + elif code == grpc.StatusCode.UNAUTHENTICATED: + fail(f'UNAUTHENTICATED -- token or metadata headers rejected: {error[0].details()}') + elif code == grpc.StatusCode.PERMISSION_DENIED: + fail(f'PERMISSION_DENIED -- tenant/fleet not authorised: {error[0].details()}') + else: + warn(f'{code.name}: {error[0].details()}') + else: + ok(f'Stream opened successfully') + + if received: + for msg in received: + field = msg.WhichOneof('payload') + ok(f'Received {field}: {getattr(msg, field)}') + else: + warn('No messages received (stream opened but server sent nothing -- may be normal)') + + +# --------------------------------------------------------------------------- +# 4. ClientStream +# --------------------------------------------------------------------------- + +def test_client_stream(channel, metadata): + section('4. ClientStream (client.ClientService)') + import grpc + from client import stream_pb2, stream_pb2_grpc + from messages import common_pb2 + + stub = stream_pb2_grpc.ClientServiceStub(channel) + + def _messages(): + yield stream_pb2.ClientMessage( + subscription = stream_pb2.SubscriptionRequest( + type = stream_pb2.ALL, + vehicle_ids = [], + ), + metadata = common_pb2.FrontendMetadata(message_id='test-sub-001'), + ) + time.sleep(2) + + received = [] + error = [None] + + def _call(): + try: + for msg in stub.ClientStream(_messages(), metadata=metadata, timeout=5): + received.append(msg) + except grpc.RpcError as e: + error[0] = e + + t = threading.Thread(target=_call) + t.start() + t.join(timeout=6) + + if error[0]: + code = error[0].code() + if code == grpc.StatusCode.UNIMPLEMENTED: + warn('UNIMPLEMENTED -- ClientService not active on this endpoint') + elif code == grpc.StatusCode.UNAUTHENTICATED: + fail(f'UNAUTHENTICATED: {error[0].details()}') + elif code == grpc.StatusCode.PERMISSION_DENIED: + fail(f'PERMISSION_DENIED: {error[0].details()}') + else: + warn(f'{code.name}: {error[0].details()}') + else: + ok('Stream opened successfully') + + if received: + for msg in received: + field = msg.WhichOneof('payload') + ok(f'Received {field}: {getattr(msg, field)}') + else: + warn('No messages received (may be normal if no vehicles are active)') + + +# --------------------------------------------------------------------------- +# Entry point +# --------------------------------------------------------------------------- + +if __name__ == '__main__': + auth_endpoint = os.environ.get('AUTH_ENDPOINT', '') + grpc_endpoint = os.environ.get('GRPC_ENDPOINT', '') + client_id = os.environ.get('CLIENT_ID', '') + client_secret = os.environ.get('CLIENT_SECRET', '') + tenant_id = os.environ.get('TENANT_ID', '') + fleet_ids = [f.strip() for f in os.environ.get('FLEET_IDS', '').split(',') if f.strip()] + + missing = [k for k, v in { + 'AUTH_ENDPOINT': auth_endpoint, + 'GRPC_ENDPOINT': grpc_endpoint, + 'CLIENT_ID': client_id, + 'CLIENT_SECRET': client_secret, + }.items() if not v] + + if missing: + print(f'Missing env vars: {", ".join(missing)}') + print('Run: set -a && source .secrets.env && set +a') + sys.exit(1) + + print(f'Testing connection to {grpc_endpoint}') + print(f'Auth: {auth_endpoint}') + print(f'Tenant: {tenant_id} Fleets: {fleet_ids}') + + token = test_token_fetch(auth_endpoint, client_id, client_secret, tenant_id, fleet_ids) + channel, metadata = test_channel(grpc_endpoint, token, tenant_id, fleet_ids) + test_vehicle_stream(channel, metadata) + test_client_stream(channel, metadata) + + print(f'\n{_GREEN}Done.{_RESET}') + channel.close() diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_frontend_client.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_frontend_client.py new file mode 100644 index 00000000..561f07e2 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_frontend_client.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 +""" +Simulates a frontend client connecting to the bridge. + +Subscribes to ALL telemetry and notifications, prints what it receives, +and optionally sends a test subscription request. + +Usage: + python3 scripts/test_frontend_client.py [--address localhost:50051] +""" +import argparse +import sys +import os +import time + +_HERE = os.path.dirname(os.path.abspath(__file__)) +sys.path.insert(0, os.path.join(_HERE, '..', 'proto', 'generated')) + +import grpc +from client import stream_pb2, stream_pb2_grpc +from messages import common_pb2 + +parser = argparse.ArgumentParser() +parser.add_argument('--address', default='localhost:50051') +args = parser.parse_args() + + +def outgoing(): + yield stream_pb2.ClientMessage( + subscription = stream_pb2.SubscriptionRequest( + type = stream_pb2.ALL, + vehicle_ids = [], + ), + metadata = common_pb2.FrontendMetadata(message_id='sub-001'), + ) + # Keep the send stream open without sending more messages. + while True: + time.sleep(30) + + +with grpc.insecure_channel(args.address) as channel: + stub = stream_pb2_grpc.ClientServiceStub(channel) + print(f'Connected to {args.address}') + try: + for server_msg in stub.ClientStream(outgoing()): + field = server_msg.WhichOneof('payload') + print(f'[server -> client] {field}: {getattr(server_msg, field)}') + except KeyboardInterrupt: + pass + except grpc.RpcError as e: + print(f'RPC error: {e.details()}') diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_integration.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_integration.py new file mode 100755 index 00000000..6316466f --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_integration.py @@ -0,0 +1,463 @@ +#!/usr/bin/env python3 +""" +Integration tests -- Supervision External Gateway gTA Integration Guide Phase 1. + +Covers all 7 test scenarios from section 8 of the spec. + +Usage: + set -a && source .secrets.env && set +a + PYTHONPATH=proto/generated python3 scripts/test_integration.py [--scenario N] +""" + +import argparse +import json +import os +import sys +import threading +import time +import urllib.error +import urllib.parse +import urllib.request + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'proto', 'generated')) + +import grpc +from client import stream_pb2, stream_pb2_grpc +from messages import common_pb2 + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +_GREEN = '\033[92m' +_RED = '\033[91m' +_YELLOW = '\033[93m' +_RESET = '\033[0m' + +_results = [] + +def _pass(scenario, msg): + print(f' {_GREEN}PASS{_RESET} {msg}') + _results.append((scenario, True, msg)) + +def _fail(scenario, msg): + print(f' {_RED}FAIL{_RESET} {msg}') + _results.append((scenario, False, msg)) + +def _info(msg): + print(f' {msg}') + + +def _fetch_token(client_id, client_secret, expect_success=True): + params = urllib.parse.urlencode({ + 'grant_type': 'client_credentials', + 'client_id': client_id, + 'client_secret': client_secret, + }).encode() + req = urllib.request.Request( + os.environ['AUTH_ENDPOINT'], + data = params, + headers = {'Content-Type': 'application/x-www-form-urlencoded'}, + method = 'POST', + ) + try: + with urllib.request.urlopen(req, timeout=30) as resp: + return resp.status, json.loads(resp.read()) + except urllib.error.HTTPError as e: + return e.code, json.loads(e.read()) + + +def _make_channel(token): + creds = grpc.composite_channel_credentials( + grpc.ssl_channel_credentials(), + grpc.metadata_call_credentials( + lambda ctx, cb: cb([('authorization', f'Bearer {token}')], None), + name='bearer', + ), + ) + return grpc.secure_channel(os.environ['GRPC_ENDPOINT'], creds) + + +def _open_stream(channel, messages_fn, timeout=5): + stub = stream_pb2_grpc.ClientServiceStub(channel) + received = [] + error = [None] + call = [None] + + def _run(): + try: + call[0] = stub.ClientStream(messages_fn()) + for msg in call[0]: + received.append(msg) + except grpc.RpcError as e: + error[0] = e + except StopIteration: + pass + + t = threading.Thread(target=_run, daemon=True) + t.start() + t.join(timeout=timeout) + if t.is_alive() and call[0]: + call[0].cancel() + t.join(timeout=2) + # Ignore cancellation errors -- those are expected from our own cancel. + if error[0] and error[0].code() == grpc.StatusCode.CANCELLED: + error[0] = None + return received, error[0] + + +def _subscription_msg(sub_type=stream_pb2.ALL, vehicle_ids=None): + def _gen(): + yield stream_pb2.ClientMessage( + subscription=stream_pb2.SubscriptionRequest( + type=sub_type, + vehicle_ids=vehicle_ids or [], + ), + metadata=common_pb2.FrontendMetadata(message_id='test-001'), + ) + # Keep the send side open so the server keeps streaming back. + # The bidi stream closes when the caller's timeout fires or the thread is stopped. + while True: + time.sleep(1) + return _gen + + +# --------------------------------------------------------------------------- +# Scenario 1: Successful token retrieval +# --------------------------------------------------------------------------- + +def scenario_1(): + print('\nScenario 1: Successful token retrieval') + status, data = _fetch_token(os.environ['CLIENT_ID'], os.environ['CLIENT_SECRET']) + _info(f'HTTP {status}') + + if status != 200: + _fail(1, f'Expected HTTP 200, got {status}: {data}') + return None + + if 'access_token' not in data: + _fail(1, 'Response missing access_token') + return None + + expires_in = data.get('expires_in') + if expires_in != 1800: + _fail(1, f'Expected expires_in=1800, got {expires_in}') + return None + + _pass(1, f'HTTP 200, access_token present, expires_in={expires_in}') + + # Decode JWT claims for visibility (no signature verification). + try: + import base64 + payload = data['access_token'].split('.')[1] + payload += '=' * (4 - len(payload) % 4) + claims = json.loads(base64.urlsafe_b64decode(payload)) + _info(f"JWT claims: { {k: claims[k] for k in ('client_id','tenant_id','fleet_ids','exp') if k in claims} }") + except Exception: + pass + + return data['access_token'] + + +# --------------------------------------------------------------------------- +# Scenario 2: Failed token retrieval (invalid credentials) +# --------------------------------------------------------------------------- + +def scenario_2(): + print('\nScenario 2: Failed token retrieval -- invalid credentials') + status, data = _fetch_token(os.environ['CLIENT_ID'], 'invalid-secret-xxxx') + _info(f'HTTP {status}: {data}') + + if status == 401: + _pass(2, f'HTTP 401 Unauthorized as expected') + else: + _fail(2, f'Expected HTTP 401, got {status}') + + +# --------------------------------------------------------------------------- +# Scenario 3: Expired token rejected +# --------------------------------------------------------------------------- + +def scenario_3(valid_token): + print('\nScenario 3: Expired token rejected') + _info('Using a deliberately malformed/expired token string') + + expired_token = valid_token[:-10] + 'XXXXXXXXXXX' + channel = _make_channel(expired_token) + _, error = _open_stream(channel, _subscription_msg(), timeout=5) + channel.close() + + if error and error.code() == grpc.StatusCode.UNAUTHENTICATED: + _pass(3, f'UNAUTHENTICATED as expected: {error.details()!r}') + elif error: + _fail(3, f'Expected UNAUTHENTICATED, got {error.code().name}: {error.details()!r}') + else: + _fail(3, 'Stream opened with invalid token -- expected rejection') + + +# --------------------------------------------------------------------------- +# Scenario 4: Telemetry subscription (happy path) +# --------------------------------------------------------------------------- + +def scenario_4(valid_token): + print('\nScenario 4: Telemetry subscription (happy path)') + channel = _make_channel(valid_token) + received, error = _open_stream(channel, _subscription_msg(stream_pb2.TELEMETRY), timeout=8) + channel.close() + + if error and error.code() not in (grpc.StatusCode.DEADLINE_EXCEEDED,): + _fail(4, f'{error.code().name}: {error.details()!r}') + return + + telemetry_msgs = [m for m in received if m.WhichOneof('payload') == 'telemetry'] + _info(f'Received {len(received)} messages total, {len(telemetry_msgs)} telemetry') + + if telemetry_msgs: + t = telemetry_msgs[0].telemetry + _info(f'vehicle_id={t.vehicle_id} connected={t.is_connected} ' + f'state={t.telemetry.state} velocity={t.telemetry.velocity:.2f}') + _pass(4, f'Stream opened, received {len(telemetry_msgs)} VehicleTelemetryUpdate message(s)') + elif not error: + _pass(4, 'Stream opened successfully (no vehicles active -- no telemetry received)') + else: + _fail(4, 'Stream timed out with no messages') + + +# --------------------------------------------------------------------------- +# Scenario 5: Vehicle ID filtering +# --------------------------------------------------------------------------- + +def scenario_5(valid_token): + print('\nScenario 5: Vehicle ID filtering') + + # First get all telemetry to find an active vehicle ID. + channel = _make_channel(valid_token) + received, _ = _open_stream(channel, _subscription_msg(stream_pb2.TELEMETRY), timeout=6) + channel.close() + + telemetry_msgs = [m for m in received if m.WhichOneof('payload') == 'telemetry'] + if not telemetry_msgs: + _info('No active vehicles found -- skipping filter validation, testing with dummy ID') + vehicle_ids = ['MV-NONEXISTENT'] + else: + vehicle_ids = list({m.telemetry.vehicle_id for m in telemetry_msgs})[:1] + _info(f'Filtering to vehicle_ids={vehicle_ids}') + + channel = _make_channel(valid_token) + filtered, error = _open_stream( + channel, _subscription_msg(stream_pb2.TELEMETRY, vehicle_ids), timeout=6) + channel.close() + + if error and error.code() not in (grpc.StatusCode.DEADLINE_EXCEEDED,): + _fail(5, f'{error.code().name}: {error.details()!r}') + return + + wrong_vehicle = [ + m for m in filtered + if m.WhichOneof('payload') == 'telemetry' + and m.telemetry.vehicle_id not in vehicle_ids + ] + + if wrong_vehicle: + _fail(5, f'Received telemetry for unrequested vehicles: ' + f'{[m.telemetry.vehicle_id for m in wrong_vehicle]}') + else: + _pass(5, f'All {len(filtered)} messages matched requested vehicle_ids={vehicle_ids}') + + +# --------------------------------------------------------------------------- +# Scenario 6: Disconnect and reconnect +# --------------------------------------------------------------------------- + +def scenario_6(valid_token): + print('\nScenario 6: Disconnect and reconnect') + + channel = _make_channel(valid_token) + stub = stream_pb2_grpc.ClientServiceStub(channel) + + def _gen(): + yield stream_pb2.ClientMessage( + subscription=stream_pb2.SubscriptionRequest(type=stream_pb2.ALL), + metadata=common_pb2.FrontendMetadata(message_id='reconn-001'), + ) + time.sleep(1) + + first_received = [] + try: + for msg in stub.ClientStream(_gen(), timeout=3): + first_received.append(msg) + except (grpc.RpcError, StopIteration): + pass + + channel.close() + _info(f'First connection: received {len(first_received)} messages, disconnected') + + # Reconnect with a fresh channel. + time.sleep(0.5) + channel2 = _make_channel(valid_token) + received2, error2 = _open_stream(channel2, _subscription_msg(stream_pb2.ALL), timeout=5) + channel2.close() + + if error2 and error2.code() not in (grpc.StatusCode.DEADLINE_EXCEEDED,): + _fail(6, f'Reconnect failed: {error2.code().name}: {error2.details()!r}') + else: + _pass(6, f'Reconnect successful, received {len(received2)} messages on second connection') + + +# --------------------------------------------------------------------------- +# Scenario 7: Invalid subscription request +# --------------------------------------------------------------------------- + +def scenario_7(valid_token): + print('\nScenario 7: Invalid subscription request') + + channel = _make_channel(valid_token) + + def _bad_request(): + # Send a ClientMessage with no subscription set (empty message). + yield stream_pb2.ClientMessage( + metadata=common_pb2.FrontendMetadata(message_id='bad-001'), + ) + time.sleep(2) + + received, error = _open_stream(channel, _bad_request, timeout=5) + channel.close() + + if error and error.code() == grpc.StatusCode.INVALID_ARGUMENT: + _pass(7, f'INVALID_ARGUMENT as expected: {error.details()!r}') + elif error: + _info(f'Got {error.code().name}: {error.details()!r}') + # Server may silently ignore rather than reject -- note it but don't hard-fail. + _pass(7, f'Server responded with {error.code().name} (acceptable -- no crash)') + else: + _info(f'Stream completed without error, received {len(received)} messages') + _pass(7, 'Server accepted message without error (lenient validation)') + + +# --------------------------------------------------------------------------- +# Entry point +# --------------------------------------------------------------------------- + + +# --------------------------------------------------------------------------- +# Inspect mode -- subscribe and pretty-print all incoming messages +# --------------------------------------------------------------------------- + +def inspect(duration=30, vehicle_ids=None, sub_type=stream_pb2.ALL): + from google.protobuf.json_format import MessageToJson + + status, data = _fetch_token(os.environ['CLIENT_ID'], os.environ['CLIENT_SECRET']) + if status != 200 or 'access_token' not in data: + print(f'Token fetch failed: {data}') + sys.exit(1) + token = data['access_token'] + + channel = _make_channel(token) + stub = stream_pb2_grpc.ClientServiceStub(channel) + + type_name = {stream_pb2.TELEMETRY: 'TELEMETRY', + stream_pb2.NOTIFICATIONS: 'NOTIFICATIONS', + stream_pb2.ALL: 'ALL'}[sub_type] + + print(f'Subscribing ({type_name}, vehicles={vehicle_ids or "all"}) ' + f'for {duration}s -- Ctrl-C to stop\n') + + counts = {} + call = [None] + + def _gen(): + yield stream_pb2.ClientMessage( + subscription=stream_pb2.SubscriptionRequest( + type=sub_type, + vehicle_ids=vehicle_ids or [], + ), + metadata=common_pb2.FrontendMetadata(message_id='inspect-001'), + ) + deadline = time.time() + duration + while time.time() < deadline: + time.sleep(0.5) + + try: + call[0] = stub.ClientStream(_gen()) + for msg in call[0]: + field = msg.WhichOneof('payload') + if field is None: + continue + + counts[field] = counts.get(field, 0) + 1 + ts = time.strftime('%H:%M:%S') + payload = getattr(msg, field) + print(f'[{ts}] {field.upper()}') + print(MessageToJson(payload, preserving_proto_field_name=True, indent=2)) + print() + + except KeyboardInterrupt: + if call[0]: + call[0].cancel() + except grpc.RpcError as e: + if e.code() != grpc.StatusCode.CANCELLED: + print(f'\nStream error: {e.code().name}: {e.details()}') + + channel.close() + print(f'\nReceived: {counts if counts else "nothing"}') + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument('--scenario', type=int, default=0, + help='Run a single scenario (1-7). Default: run all.') + parser.add_argument('--inspect', action='store_true', + help='Subscribe and print all incoming messages.') + parser.add_argument('--duration', type=int, default=30, + help='Inspect duration in seconds (default: 30).') + parser.add_argument('--vehicle', nargs='*', default=None, + help='Filter to specific vehicle IDs during inspect.') + parser.add_argument('--type', choices=['ALL','TELEMETRY','NOTIFICATIONS'], + default='ALL', dest='sub_type', + help='Subscription type for inspect (default: ALL).') + args = parser.parse_args() + + for var in ('AUTH_ENDPOINT', 'GRPC_ENDPOINT', 'CLIENT_ID', 'CLIENT_SECRET'): + if not os.environ.get(var): + print(f'Missing env var: {var}') + print('Run: set -a && source .secrets.env && set +a') + sys.exit(1) + + print(f'Auth: {os.environ["AUTH_ENDPOINT"]}') + print(f'gRPC: {os.environ["GRPC_ENDPOINT"]}') + print(f'Client: {os.environ["CLIENT_ID"]}') + + if args.inspect: + sub_type_map = {'ALL': stream_pb2.ALL, 'TELEMETRY': stream_pb2.TELEMETRY, + 'NOTIFICATIONS': stream_pb2.NOTIFICATIONS} + inspect(duration=args.duration, vehicle_ids=args.vehicle, + sub_type=sub_type_map[args.sub_type]) + sys.exit(0) + + token = scenario_1() if args.scenario in (0, 1) else None + + if args.scenario == 0 or args.scenario != 1: + if token is None: + token = _fetch_token(os.environ['CLIENT_ID'], os.environ['CLIENT_SECRET'])[1].get('access_token') + if not token: + print('Cannot obtain token -- aborting remaining tests') + sys.exit(1) + + run = lambda n, fn, *a: fn(*a) if args.scenario in (0, n) else None + + run(2, scenario_2) + run(3, scenario_3, token) + run(4, scenario_4, token) + run(5, scenario_5, token) + run(6, scenario_6, token) + run(7, scenario_7, token) + + if args.scenario == 0: + passed = sum(1 for _, ok, _ in _results if ok) + failed = sum(1 for _, ok, _ in _results if not ok) + print(f'\n{"="*50}') + print(f'Results: {_GREEN}{passed} passed{_RESET} {_RED}{failed} failed{_RESET} ' + f'of {len(_results)} checks') + if failed: + sys.exit(1) + diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_vehicle_client.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_vehicle_client.py new file mode 100644 index 00000000..d550fbb3 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_vehicle_client.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 +""" +Simulates a vehicle connecting to the bridge as a gRPC client. + +Streams VehicleMessage telemetry/heartbeat to the bridge and prints any +ServerMessage commands it receives back. + +Usage: + python3 scripts/test_vehicle_client.py [--address localhost:50051] +""" +import argparse +import sys +import time +import threading +import os + +# Add generated stubs to path. +_HERE = os.path.dirname(os.path.abspath(__file__)) +sys.path.insert(0, os.path.join(_HERE, '..', 'proto', 'generated')) + +import grpc +from vehicle import stream_pb2, stream_pb2_grpc +from messages import telemetry_pb2, events_pb2, common_pb2 + +parser = argparse.ArgumentParser() +parser.add_argument('--address', default='localhost:50051') +parser.add_argument('--interval', type=float, default=1.0, help='Telemetry publish interval (s)') +args = parser.parse_args() + + +def message_stream(): + seq = 0 + while True: + telemetry = telemetry_pb2.Telemetry( + state = common_pb2.NOMINAL_DRIVING, + heading = 1.57, + velocity = 30.0, + battery = 87.5, + passengers = 2, + ) + yield stream_pb2.VehicleMessage( + telemetry = telemetry, + metadata = common_pb2.MessageMetadata( + message_id = f'msg-{seq}', + vehicle_id = 'vehicle-001', + ), + ) + + if seq % 5 == 0: + yield stream_pb2.VehicleMessage( + heartbeat = events_pb2.Heartbeat(status='ok'), + metadata = common_pb2.MessageMetadata(vehicle_id='vehicle-001'), + ) + + seq += 1 + time.sleep(args.interval) + + +with grpc.insecure_channel(args.address) as channel: + stub = stream_pb2_grpc.VehicleServiceStub(channel) + print(f'Connected to {args.address}') + try: + for server_msg in stub.VehicleStream(message_stream()): + field = server_msg.WhichOneof('payload') + print(f'[server -> vehicle] {field}: {getattr(server_msg, field)}') + except KeyboardInterrupt: + pass + except grpc.RpcError as e: + print(f'RPC error: {e.details()}') diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grps_oath_test.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grps_oath_test.sh new file mode 100644 index 00000000..14642be5 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grps_oath_test.sh @@ -0,0 +1,3 @@ +curl -v -X POST https://supervision.dev-motor-ai.com/auth/token \ + -H "Content-Type: application/x-www-form-urlencoded" \ + -d "grant_type=client_credentials&client_id=${CLIENT_ID}&client_secret=${CLIENT_SECRET}&tenant_id=${TENANT_ID}&fleet_ids=${FLEET_IDS}" diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/bridge_mqtt_args.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/bridge_mqtt_args.py new file mode 100644 index 00000000..a3bcc835 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/bridge_mqtt_args.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +import os +import sys +import yaml + + +def env_or(cfg, key, default=None, env_var=None): + name = cfg.get(f'{key}_env') or env_var + if name: + val = os.environ.get(name) + if val is not None: + return val + return cfg.get(key, default) + + +def load_env_file(path): + if not path or not os.path.exists(path): + return + with open(path) as f: + for line in f: + line = line.strip() + if not line or line.startswith('#') or '=' not in line: + continue + key, _, value = line.partition('=') + os.environ.setdefault(key.strip(), value.strip().strip('"').strip("'")) + + +def main(): + config_path = sys.argv[1] + with open(config_path) as f: + cfg = yaml.safe_load(f) or {} + mqtt = cfg.get('mqtt', {}) + + env_file = mqtt.get('env_file') + if env_file and not os.path.isabs(env_file): + env_file = os.path.join(os.path.dirname(os.path.abspath(config_path)), env_file) + load_env_file(env_file) + + cert_dir = os.environ.get('MQTT_BRIDGE_CERT_DIR', '') + + def resolve(path): + if not path or os.path.isabs(path): + return path + return os.path.join(cert_dir, path) + + args = ['-h', str(env_or(mqtt, 'host', 'localhost', 'MQTT_HOST')), + '-p', str(env_or(mqtt, 'port', 1883, 'MQTT_PORT'))] + + auth = mqtt.get('auth') or {} + user_env = auth.get('username_env', 'MQTT_USERNAME') + pass_env = auth.get('password_env', 'MQTT_PASSWORD') + username = os.environ.get(user_env) if user_env else None + password = os.environ.get(pass_env) if pass_env else None + if username: + args += ['-u', username] + if password: + args += ['-P', password] + + tls = mqtt.get('tls') or {} + enabled = env_or(tls, 'enabled', False, 'MQTT_TLS') + if isinstance(enabled, str): + enabled = enabled.lower() in ('1', 'true', 'yes') + if enabled: + ca_certs = resolve(env_or(tls, 'ca_certs', env_var='MQTT_CA_CERT')) + certfile = resolve(env_or(tls, 'certfile', env_var='MQTT_CLIENT_CERT')) + keyfile = resolve(env_or(tls, 'keyfile', env_var='MQTT_CLIENT_KEY')) + if ca_certs: + args += ['--cafile', ca_certs] + else: + args += ['--capath', '/etc/ssl/certs'] + if certfile: + args += ['--cert', certfile] + if keyfile: + args += ['--key', keyfile] + if tls.get('insecure'): + args += ['--insecure'] + + for arg in args: + sys.stdout.write(arg + '\0') + + +if __name__ == '__main__': + main() diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh index 95d291dd..07de09ae 100755 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh @@ -1,46 +1,32 @@ #!/usr/bin/env bash -# Sourced by other scripts. Reads broker settings from a .env file if provided, -# then falls back to environment variables, then to defaults. +# Sourced by other scripts. Resolves connection settings from bridge_config.yaml +# (the single source of truth) and exposes them as the _broker_args array. +# Precedence: real environment > env_file in the config > config defaults. # -# Usage: source mqtt_common.sh [/path/to/mqtt.env] +# Usage: source mqtt_common.sh [/path/to/bridge_config.yaml] +# Defaults to /bridge_config.yaml. -_env_file="${1:-${MQTT_ENV_FILE:-}}" +_COMMON_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +_PKG_ROOT="$(cd "$_COMMON_DIR/.." && pwd)" +export MQTT_BRIDGE_CERT_DIR="${MQTT_BRIDGE_CERT_DIR:-$_PKG_ROOT/certs}" -if [[ -n "$_env_file" ]]; then - if [[ ! -f "$_env_file" ]]; then - echo "ERROR: env file not found: $_env_file" >&2 - exit 1 - fi - set -a - # shellcheck disable=SC1090 - source "$_env_file" - set +a +_config="${1:-$_PKG_ROOT/bridge_config.yaml}" +if [[ ! -f "$_config" ]]; then + echo "ERROR: bridge config not found: $_config" >&2 + exit 1 fi -MQTT_HOST="${MQTT_HOST:-localhost}" -MQTT_PORT="${MQTT_PORT:-1883}" -MQTT_USERNAME="${MQTT_USERNAME:-}" -MQTT_PASSWORD="${MQTT_PASSWORD:-}" -# TLS: set MQTT_TLS=1 to enable. Provide MQTT_CA_CERT for a custom CA, -# or leave unset to use the system CA store (/etc/ssl/certs). -MQTT_TLS="${MQTT_TLS:-}" -MQTT_CA_CERT="${MQTT_CA_CERT:-}" - -_auth_args=() -if [[ -n "$MQTT_USERNAME" ]]; then - _auth_args+=(-u "$MQTT_USERNAME") -fi -if [[ -n "$MQTT_PASSWORD" ]]; then - _auth_args+=(-P "$MQTT_PASSWORD") -fi - -_tls_args=() -if [[ -n "$MQTT_TLS" ]]; then - if [[ -n "$MQTT_CA_CERT" ]]; then - _tls_args+=(--cafile "$MQTT_CA_CERT") - else - _tls_args+=(--capath /etc/ssl/certs) - fi +mapfile -d '' _broker_args < <(python3 "$_COMMON_DIR/bridge_mqtt_args.py" "$_config") +if [[ ${#_broker_args[@]} -eq 0 ]]; then + echo "ERROR: failed to parse bridge config: $_config" >&2 + exit 1 fi -_broker_args=(-h "$MQTT_HOST" -p "$MQTT_PORT" "${_auth_args[@]}" "${_tls_args[@]}") +MQTT_HOST=""; MQTT_PORT=""; MQTT_USERNAME="" +for ((_i = 0; _i < ${#_broker_args[@]}; _i++)); do + case "${_broker_args[_i]}" in + -h) MQTT_HOST="${_broker_args[_i + 1]}" ;; + -p) MQTT_PORT="${_broker_args[_i + 1]}" ;; + -u) MQTT_USERNAME="${_broker_args[_i + 1]}" ;; + esac +done diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log.sh new file mode 100644 index 00000000..f032cb31 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log.sh @@ -0,0 +1,48 @@ +#!/usr/bin/env bash +# Subscribes to one or more topics and appends each message to a JSON Lines file. +# Connection settings come from bridge_config.yaml (same as the bridge). +# +# Usage: ./mqtt_log.sh [/path/to/bridge_config.yaml] [-o OUTFILE] [topic ...] +# +# Examples: +# ./mqtt_log.sh # all topics -> mqtt_log_.jsonl +# ./mqtt_log.sh -o solbox.jsonl 'od_imoger/solbox/+/notifications' +# ./mqtt_log.sh bridge_config.yaml -o run.jsonl 'od_imoger/#' + +set -euo pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +_config="" +if [[ "${1:-}" == *.yaml || "${1:-}" == *.yml ]]; then + _config="$1" + shift +fi + +OUTFILE="mqtt_log_$(date '+%Y%m%d_%H%M%S').jsonl" +if [[ "${1:-}" == "-o" ]]; then + OUTFILE="$2" + shift 2 +fi + +source "$SCRIPT_DIR/mqtt_common.sh" "$_config" + +TOPICS=("$@") +if [[ ${#TOPICS[@]} -eq 0 ]]; then + TOPICS=('#') +fi + +_topic_args=() +for t in "${TOPICS[@]}"; do + _topic_args+=(-t "$t") +done + +echo "=== MQTT Logger ===" +echo "Broker : $MQTT_HOST:$MQTT_PORT" +echo "Topics : ${TOPICS[*]}" +echo "Output : $OUTFILE" +echo "Press Ctrl+C to stop." +echo "" + +# -F %j: topic-then-payload separated by a tab, payload raw, one line per message. +mosquitto_sub "${_broker_args[@]}" "${_topic_args[@]}" -F '%t %p' \ + | python3 "$SCRIPT_DIR/mqtt_log_writer.py" "$OUTFILE" diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log_writer.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log_writer.py new file mode 100644 index 00000000..364b73a7 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log_writer.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +import json +import sys +from datetime import datetime, timezone + + +def main(): + out_path = sys.argv[1] + with open(out_path, 'a', buffering=1) as out: + for line in sys.stdin: + line = line.rstrip('\n') + if not line: + continue + topic, _, payload = line.partition('\t') + try: + payload_val = json.loads(payload) + except (ValueError, TypeError): + payload_val = payload + record = { + 'ts': datetime.now(timezone.utc).isoformat(), + 'topic': topic, + 'payload': payload_val, + } + out.write(json.dumps(record, ensure_ascii=False) + '\n') + + +if __name__ == '__main__': + try: + main() + except (BrokenPipeError, KeyboardInterrupt): + pass From da373f36ec3fbf2aa10c6f9dbe1eab5b64e6eb50 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Wed, 1 Jul 2026 11:58:43 +0200 Subject: [PATCH 05/18] added grpc message bridge Signed-off-by: Andrew Koerner --- .../grpc_message_bridge/.gitignore | 1 + .../grpc_message_bridge/Dockerfile | 26 ++-- .../grpc_message_bridge/Makefile | 124 +++++++++++++----- .../config/bridge_config.yaml | 44 +------ .../grpc_message_bridge/docker-entrypoint.sh | 15 ++- .../launch/bridge.launch.py | 32 +++-- .../grpc_message_bridge/package.xml | 17 ++- .../scripts/test_integration.py | 36 ++++- .../grpc_message_bridge/setup.py | 31 +++-- 9 files changed, 187 insertions(+), 139 deletions(-) mode change 100755 => 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-entrypoint.sh diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore index 9551e2ae..8254e732 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore @@ -1 +1,2 @@ .secrets.env +proto/generated diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Dockerfile b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Dockerfile index 7b58f6a5..eea14eb5 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Dockerfile +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Dockerfile @@ -1,31 +1,23 @@ ARG ROS_DISTRO=jazzy -ARG OS_CODE_NAME=noble -FROM ros:${ROS_DISTRO}-ros-core-${OS_CODE_NAME} AS base -ARG ROS_DISTRO=jazzy -ENV ROS_DISTRO=${ROS_DISTRO} -ENV DEBIAN_FRONTEND=noninteractive +FROM ros:${ROS_DISTRO}-ros-base RUN apt-get update && apt-get install -y --no-install-recommends \ python3-pip \ - python3-colcon-common-extensions \ - ros-${ROS_DISTRO}-rosidl-runtime-py \ - ros-${ROS_DISTRO}-std-msgs \ + python3-grpcio \ + python3-yaml \ && rm -rf /var/lib/apt/lists/* -WORKDIR /ros2_ws/src/grpc_message_bridge - -COPY . . +RUN pip3 install --no-cache-dir --break-system-packages grpcio-tools protobuf -RUN pip3 install --no-cache-dir --break-system-packages -r requirements.pip3 +WORKDIR /ws +COPY . /ws/src/grpc_message_bridge/ -WORKDIR /ros2_ws +ENV PYTHONPATH=/ws/src/grpc_message_bridge/proto/generated RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \ - colcon build --packages-select grpc_message_bridge --symlink-install - -ENV RMW_IMPLEMENTATION=rmw_fastrtps_cpp + cd /ws && \ + colcon build --packages-select grpc_message_bridge COPY docker-entrypoint.sh /docker-entrypoint.sh RUN chmod +x /docker-entrypoint.sh - ENTRYPOINT ["/docker-entrypoint.sh"] diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile index 4e17af90..baf8cb28 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile @@ -1,46 +1,79 @@ +PACKAGE ?= grpc_message_bridge +PROTO_ROOT ?= $(PWD)/proto +PROTO_OUT ?= $(PWD)/proto/generated + +PROTO_SOURCES := \ + $(PROTO_ROOT)/messages/common.proto \ + $(PROTO_ROOT)/messages/events.proto \ + $(PROTO_ROOT)/messages/telemetry.proto \ + $(PROTO_ROOT)/messages/signaling.proto \ + $(PROTO_ROOT)/messages/commands.proto \ + $(PROTO_ROOT)/messages/config/sensors.proto \ + $(PROTO_ROOT)/messages/config/vehicle.proto \ + $(PROTO_ROOT)/vehicle/stream.proto \ + $(PROTO_ROOT)/client/notifications.proto \ + $(PROTO_ROOT)/client/stream.proto + IMAGE_NAME ?= grpc_message_bridge IMAGE_TAG ?= latest ROS_DISTRO ?= jazzy -OS_CODE_NAME ?= noble - +GRPC_HOST ?= 0.0.0.0 +GRPC_PORT ?= 50051 BRIDGE_CONFIG ?= $(PWD)/config/bridge_config.yaml - -GRPC_HOST ?= 0.0.0.0 -GRPC_PORT ?= 50051 +SECRETS_ENV ?= $(PWD)/.secrets.env .PHONY: help help: - @echo "Usage: make " - @echo "" @echo "Targets:" - @echo " build Build the bridge Docker image" - @echo " run Start the bridge node container" - @echo " stop Stop and remove running containers" - @echo " logs Tail bridge container logs" - @echo " clean Stop containers, remove image and prune build cache" - @echo " gen_proto Regenerate Python stubs from ros_bridge.proto" - @echo "" - @echo "Overridable variables:" - @echo " IMAGE_NAME $(IMAGE_NAME)" - @echo " IMAGE_TAG $(IMAGE_TAG)" - @echo " ROS_DISTRO $(ROS_DISTRO)" - @echo " OS_CODE_NAME $(OS_CODE_NAME)" - @echo " BRIDGE_CONFIG $(BRIDGE_CONFIG)" - @echo " GRPC_HOST $(GRPC_HOST)" - @echo " GRPC_PORT $(GRPC_PORT)" + @echo " gen_proto Regenerate Python stubs from proto/" + @echo " build Build Docker image" + @echo " start Start bridge container (alias for run)" + @echo " run Start bridge container" + @echo " stop Stop bridge container" + @echo " restart Stop then start" + @echo " logs Tail bridge container logs (Ctrl-C to exit)" + @echo " crash Show logs from last exited container" + @echo " test Run integration test suite inside the container" + @echo " inspect Subscribe and print live data (DURATION=30 VEHICLE=MV-001 TYPE=ALL)" + @echo " SCENARIO=N to run a single scenario" + @echo " clean Stop + remove image" + +.PHONY: gen_proto +gen_proto: + @mkdir -p \ + $(PROTO_OUT)/messages/config \ + $(PROTO_OUT)/vehicle \ + $(PROTO_OUT)/client + python3 -m grpc_tools.protoc \ + -I$(PROTO_ROOT) \ + $(shell python3 -m grpc_tools.protoc --version > /dev/null 2>&1 && \ + python3 -c "import grpc_tools,os; print('-I'+os.path.join(os.path.dirname(grpc_tools.__file__),'_proto'))") \ + --python_out=$(PROTO_OUT) \ + --grpc_python_out=$(PROTO_OUT) \ + $(PROTO_SOURCES) + @for d in $(PROTO_OUT) \ + $(PROTO_OUT)/messages \ + $(PROTO_OUT)/messages/config \ + $(PROTO_OUT)/vehicle \ + $(PROTO_OUT)/client; do \ + touch $$d/__init__.py; \ + done + @echo "Stubs written to $(PROTO_OUT)" .PHONY: build build: docker build \ --build-arg ROS_DISTRO=$(ROS_DISTRO) \ - --build-arg OS_CODE_NAME=$(OS_CODE_NAME) \ -t $(IMAGE_NAME):$(IMAGE_TAG) . -.PHONY: run -run: +.PHONY: start run +start run: + @test -f $(SECRETS_ENV) || (echo "ERROR: $(SECRETS_ENV) not found"; exit 1) + -docker stop grpc-bridge 2>/dev/null || true docker run --rm -d \ --name grpc-bridge \ --network host \ + --env-file $(SECRETS_ENV) \ -v $(BRIDGE_CONFIG):/config/bridge_config.yaml:ro \ -e BRIDGE_CONFIG_PATH=/config/bridge_config.yaml \ -e GRPC_HOST=$(GRPC_HOST) \ @@ -49,23 +82,42 @@ run: .PHONY: stop stop: - docker stop grpc-bridge 2>/dev/null || true + -docker stop grpc-bridge 2>/dev/null || true + +.PHONY: restart +restart: stop start .PHONY: logs logs: docker logs -f grpc-bridge +.PHONY: crash +crash: + docker logs $$(docker ps -aq --filter name=grpc-bridge | head -1) 2>&1 || \ + echo "No exited grpc-bridge container found" + +.PHONY: inspect +inspect: + @test -f $(SECRETS_ENV) || (echo "ERROR: $(SECRETS_ENV) not found"; exit 1) + docker run --rm -it \ + --network host \ + --env-file $(SECRETS_ENV) \ + -e PYTHONPATH=/ws/src/grpc_message_bridge/proto/generated \ + $(IMAGE_NAME):$(IMAGE_TAG) \ + python3 /ws/src/grpc_message_bridge/scripts/test_integration.py \ + --inspect --duration $(or $(DURATION),30) $(if $(VEHICLE),--vehicle $(VEHICLE),) $(if $(TYPE),--type $(TYPE),) + +.PHONY: test +test: + @test -f $(SECRETS_ENV) || (echo "ERROR: $(SECRETS_ENV) not found"; exit 1) + docker run --rm \ + --network host \ + --env-file $(SECRETS_ENV) \ + -e PYTHONPATH=/ws/src/grpc_message_bridge/proto/generated \ + $(IMAGE_NAME):$(IMAGE_TAG) \ + python3 /ws/src/grpc_message_bridge/scripts/test_integration.py + .PHONY: clean clean: stop docker rmi $(IMAGE_NAME):$(IMAGE_TAG) 2>/dev/null || true docker builder prune -f - -.PHONY: gen_proto -gen_proto: - python3 -m grpc_tools.protoc \ - -I grpc_message_bridge \ - --python_out=grpc_message_bridge \ - --grpc_python_out=grpc_message_bridge \ - grpc_message_bridge/ros_bridge.proto - sed -i 's/^import ros_bridge_pb2/from . import ros_bridge_pb2/' \ - grpc_message_bridge/ros_bridge_pb2_grpc.py diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml index 8f39a4a8..d42d5c33 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml @@ -1,41 +1,5 @@ -# ROS2 domain ID this bridge node subscribes/publishes on. -# Defaults to $ROS_DOMAIN if unset. -ros_domain_id: 0 +enabled_streams: + - client.ClientService.ClientStream -# Bridge ROS2 topics to a remote gRPC server. -# The remote end must be running a grpc_message_bridge with grpc_to_ros2 configured -# for the same topic. -ros2_to_grpc: - - ros_topic: "/ros2_chatter" - msg_type: "std_msgs/msg/String" - grpc_address: "localhost:50052" # Remote bridge gRPC server address - # format: cdr # 'cdr' (default): raw CDR bytes. - # # 'json': raw JSON bytes with 'datatype' metadata. - # # Bridge-to-bridge only. - # # 'cdr_json': CDR-serialized std_msgs/msg/String whose - # # 'data' field contains JSON with 'datatype' metadata. - # qos_reliability: reliable # 'reliable' (default) or 'best_effort' - # qos_durability: volatile # 'volatile' (default) or 'transient_local' - # qos_depth: 1 - -# Accept inbound gRPC Publish streams and forward to ROS2 topics. -# The sending end must be running a grpc_message_bridge with ros2_to_grpc configured -# for the same topic pointing at this node's grpc_port. -grpc_to_ros2: - - ros_topic: "/grpc_chatter" - msg_type: "std_msgs/msg/String" - # format: cdr - # qos_reliability: best_effort # 'best_effort' (default) or 'reliable' - # qos_durability: volatile - # qos_depth: 1 - -# Connect as a gRPC client subscriber to a remote bridge server and publish -# the received messages locally on ROS2. Complements ros2_to_grpc on the remote side. -# grpc_subscribe_from: -# - ros_topic: "/remote_chatter" -# msg_type: "std_msgs/msg/String" -# grpc_address: "localhost:50052" -# # format: cdr -# # qos_reliability: best_effort -# # qos_durability: volatile -# # qos_depth: 1 +client_streams: + - stream: client.ClientService.ClientStream diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-entrypoint.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-entrypoint.sh old mode 100755 new mode 100644 index 0a43e754..9c169d26 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-entrypoint.sh +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/docker-entrypoint.sh @@ -1,11 +1,16 @@ #!/bin/bash set -e - source /opt/ros/${ROS_DISTRO}/setup.bash -source /ros2_ws/install/setup.bash +source /ws/install/setup.bash + +# If arguments are passed, run them directly instead of the bridge node. +# This allows: docker run ... python3 scripts/test_integration.py +if [ $# -gt 0 ]; then + exec "$@" +fi exec ros2 run grpc_message_bridge bridge_node \ --ros-args \ - -p config_path:="${BRIDGE_CONFIG_PATH:-/ros2_ws/install/grpc_message_bridge/share/grpc_message_bridge/config/bridge_config.yaml}" \ - -p grpc_host:="${GRPC_HOST:-0.0.0.0}" \ - -p grpc_port:="${GRPC_PORT:-50051}" + -p config_path:=${BRIDGE_CONFIG_PATH:-/config/bridge_config.yaml} \ + -p grpc_host:=${GRPC_HOST:-0.0.0.0} \ + -p grpc_port:=${GRPC_PORT:-50051} diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/launch/bridge.launch.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/launch/bridge.launch.py index 070252f1..f512f8ab 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/launch/bridge.launch.py +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/launch/bridge.launch.py @@ -1,24 +1,28 @@ from launch import LaunchDescription -from launch_ros.actions import Node from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from ament_index_python.packages import get_package_share_directory -import os +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + def generate_launch_description(): - pkg_share = get_package_share_directory('grpc_message_bridge') - bridge_config = os.path.join(pkg_share, 'config', 'bridge_config.yaml') + pkg = FindPackageShare('grpc_message_bridge') return LaunchDescription([ + DeclareLaunchArgument('config_path', + default_value=PathJoinSubstitution([pkg, 'config', 'bridge_config.yaml'])), DeclareLaunchArgument('grpc_host', default_value='0.0.0.0'), DeclareLaunchArgument('grpc_port', default_value='50051'), + Node( - package='grpc_message_bridge', - executable='bridge_node', - parameters=[{ - 'config_path': bridge_config, - 'grpc_host': LaunchConfiguration('grpc_host'), - 'grpc_port': LaunchConfiguration('grpc_port'), - }] - ) + package = 'grpc_message_bridge', + executable = 'bridge_node', + name = 'grpc_bridge_node', + parameters = [{ + 'config_path': LaunchConfiguration('config_path'), + 'grpc_host': LaunchConfiguration('grpc_host'), + 'grpc_port': LaunchConfiguration('grpc_port'), + }], + output = 'screen', + ), ]) diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/package.xml b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/package.xml index ad673254..bddb84f8 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/package.xml +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/package.xml @@ -1,15 +1,20 @@ + grpc_message_bridge - 1.0.0 - Bridge between ROS 2 and gRPC - Eclipse ADORe + 2.0.0 + ROS 2 gRPC message bridge with bidi stream and oneof field routing + motorai Apache-2.0 + rclpy std_msgs - rosidl_runtime_py - grpcio - grpcio-tools + rcl_interfaces + + python3-grpcio + python3-grpcio-tools + python3-yaml + ament_python diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_integration.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_integration.py index 6316466f..3ecf5a35 100755 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_integration.py +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_integration.py @@ -344,7 +344,7 @@ def _bad_request(): # --------------------------------------------------------------------------- def inspect(duration=30, vehicle_ids=None, sub_type=stream_pb2.ALL): - from google.protobuf.json_format import MessageToJson + import base64 status, data = _fetch_token(os.environ['CLIENT_ID'], os.environ['CLIENT_SECRET']) if status != 200 or 'access_token' not in data: @@ -386,10 +386,36 @@ def _gen(): counts[field] = counts.get(field, 0) + 1 ts = time.strftime('%H:%M:%S') - payload = getattr(msg, field) - print(f'[{ts}] {field.upper()}') - print(MessageToJson(payload, preserving_proto_field_name=True, indent=2)) - print() + + if field == 'telemetry': + t = msg.telemetry + tel = t.telemetry + print(f'[{ts}] TELEMETRY vehicle={t.vehicle_id} connected={t.is_connected}') + print(f' state={tel.state} ' + f'pos=({tel.position.lat:.5f}, {tel.position.lon:.5f}) ' + f'heading={tel.heading:.3f}rad ' + f'velocity={tel.velocity:.2f}km/h ' + f'battery={tel.battery:.1f}% ' + f'passengers={tel.passengers} ' + f'obstacles={len(tel.obstacles)}') + + elif field == 'notification': + n = msg.notification.notification + print(f'[{ts}] NOTIFICATION id={msg.notification.id} ' + f'vehicle={msg.notification.vehicle_id}') + print(f' [{n.severity}] {n.title}: {n.message} ' + f'node={n.ros_node}') + + elif field == 'ack': + print(f'[{ts}] ACK ok={msg.ack.success} msg={msg.ack.error_msg!r}') + + elif field == 'signal_message': + s = msg.signal_message + inner = s.WhichOneof('payload') + print(f'[{ts}] SIGNAL session={s.session_id} peer={s.peer_id} type={inner}') + + else: + print(f'[{ts}] {field.upper()} {getattr(msg, field)}') except KeyboardInterrupt: if call[0]: diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py index f2b748fb..8e4aae85 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py @@ -1,25 +1,24 @@ -from setuptools import setup +from setuptools import setup, find_packages package_name = 'grpc_message_bridge' setup( - name=package_name, - version='1.0.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ('share/' + package_name + '/config', ['config/bridge_config.yaml']), - ('share/' + package_name + '/launch', ['launch/bridge.launch.py']), + name = package_name, + version = '2.0.0', + packages = find_packages(exclude=['test']), + data_files = [ + ('share/ament_index/resource_index/packages', ['resource/grpc_message_bridge']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/config', ['config/bridge_config.yaml']), + ('share/' + package_name + '/launch', ['launch/bridge.launch.py']), ], - install_requires=['setuptools', 'grpcio', 'grpcio-tools', 'pyyaml'], - zip_safe=True, - maintainer='akoerner', - description='ROS 2 gRPC Message Bridge', - license='Apache-2.0', - entry_points={ + install_requires = ['setuptools', 'grpcio', 'grpcio-tools', 'pyyaml', 'protobuf'], + zip_safe = True, + maintainer = 'motorai', + license = 'Apache-2.0', + entry_points = { 'console_scripts': [ - 'bridge_node = grpc_message_bridge.bridge_node:main' + 'bridge_node = grpc_message_bridge.bridge_node:main', ], }, ) From 6e2d12c6185c1b3369e88698190d26148c42cdd1 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Wed, 1 Jul 2026 12:36:17 +0200 Subject: [PATCH 06/18] updated .gitmodules Signed-off-by: Andrew Koerner --- .gitmodules | 5 +---- .../src/adore_interfaces/grpc_message_bridge/Makefile | 4 ++-- .../{scripts => tools}/grpc_check_server.sh | 0 .../grpc_message_bridge/{scripts => tools}/grpc_common.sh | 0 .../{scripts => tools}/grpc_test_bridge.sh | 0 .../{scripts => tools}/grpc_test_pubsub.sh | 0 .../{scripts => tools}/test_connection.py | 0 .../{scripts => tools}/test_frontend_client.py | 0 .../{scripts => tools}/test_integration.py | 0 .../{scripts => tools}/test_vehicle_client.py | 0 10 files changed, 3 insertions(+), 6 deletions(-) rename ros2_workspace/src/adore_interfaces/grpc_message_bridge/{scripts => tools}/grpc_check_server.sh (100%) rename ros2_workspace/src/adore_interfaces/grpc_message_bridge/{scripts => tools}/grpc_common.sh (100%) rename ros2_workspace/src/adore_interfaces/grpc_message_bridge/{scripts => tools}/grpc_test_bridge.sh (100%) rename ros2_workspace/src/adore_interfaces/grpc_message_bridge/{scripts => tools}/grpc_test_pubsub.sh (100%) rename ros2_workspace/src/adore_interfaces/grpc_message_bridge/{scripts => tools}/test_connection.py (100%) rename ros2_workspace/src/adore_interfaces/grpc_message_bridge/{scripts => tools}/test_frontend_client.py (100%) rename ros2_workspace/src/adore_interfaces/grpc_message_bridge/{scripts => tools}/test_integration.py (100%) rename ros2_workspace/src/adore_interfaces/grpc_message_bridge/{scripts => tools}/test_vehicle_client.py (100%) diff --git a/.gitmodules b/.gitmodules index 942a7345..ee60c45c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -92,7 +92,7 @@ url = https://github.com/Lichtblick-Suite/lichtblick.git [submodule "vendor/libOpenDRIVE/libOpenDRIVE"] path = vendor/libOpenDRIVE/libOpenDRIVE - url = git@github.com:pageldev/libOpenDRIVE.git + url = https://github.com/DLR-TS/libopendrive.git [submodule "ros2_workspace/src/vendor/osqp-eigen"] path = ros2_workspace/src/vendor/osqp_eigen url = https://github.com/robotology/osqp-eigen.git @@ -102,9 +102,6 @@ [submodule "ros2_workspace/src/vendor/osqp"] path = ros2_workspace/src/vendor/osqp/osqp url = https://github.com/osqp/osqp.git -[submodule "ros2_workspace/src/vendor/libOpenDRIVE"] - path = ros2_workspace/src/vendor/libOpenDRIVE - url = git@github.com:pageldev/libOpenDRIVE.git [submodule "ros2_workspace/src/vendor/qdldl"] path = ros2_workspace/src/vendor/osqp/qdldl url = git@github.com:osqp/qdldl.git diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile index baf8cb28..54abea01 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/Makefile @@ -104,7 +104,7 @@ inspect: --env-file $(SECRETS_ENV) \ -e PYTHONPATH=/ws/src/grpc_message_bridge/proto/generated \ $(IMAGE_NAME):$(IMAGE_TAG) \ - python3 /ws/src/grpc_message_bridge/scripts/test_integration.py \ + python3 /ws/src/grpc_message_bridge/tools/test_integration.py \ --inspect --duration $(or $(DURATION),30) $(if $(VEHICLE),--vehicle $(VEHICLE),) $(if $(TYPE),--type $(TYPE),) .PHONY: test @@ -115,7 +115,7 @@ test: --env-file $(SECRETS_ENV) \ -e PYTHONPATH=/ws/src/grpc_message_bridge/proto/generated \ $(IMAGE_NAME):$(IMAGE_TAG) \ - python3 /ws/src/grpc_message_bridge/scripts/test_integration.py + python3 /ws/src/grpc_message_bridge/tools/test_integration.py .PHONY: clean clean: stop diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_check_server.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grpc_check_server.sh similarity index 100% rename from ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_check_server.sh rename to ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grpc_check_server.sh diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_common.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grpc_common.sh similarity index 100% rename from ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_common.sh rename to ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grpc_common.sh diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_bridge.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grpc_test_bridge.sh similarity index 100% rename from ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_bridge.sh rename to ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grpc_test_bridge.sh diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_pubsub.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grpc_test_pubsub.sh similarity index 100% rename from ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/grpc_test_pubsub.sh rename to ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/grpc_test_pubsub.sh diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_connection.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_connection.py similarity index 100% rename from ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_connection.py rename to ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_connection.py diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_frontend_client.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_frontend_client.py similarity index 100% rename from ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_frontend_client.py rename to ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_frontend_client.py diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_integration.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_integration.py similarity index 100% rename from ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_integration.py rename to ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_integration.py diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_vehicle_client.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_vehicle_client.py similarity index 100% rename from ros2_workspace/src/adore_interfaces/grpc_message_bridge/scripts/test_vehicle_client.py rename to ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_vehicle_client.py From 9debe3728fe6ed229fd117c2be3c06e24ea1c4ff Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Wed, 1 Jul 2026 12:55:03 +0200 Subject: [PATCH 07/18] updated .gitmodules, changed libopendrive to dlr remote for offline patch Signed-off-by: Andrew Koerner --- .gitmodules | 3 +++ ros2_workspace/src/vendor/libOpenDRIVE | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/.gitmodules b/.gitmodules index ee60c45c..866844f5 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,6 +7,9 @@ [submodule "mission_control"] path = ros2_workspace/src/adore_ros2_nodes/mission_control url = git@github.com:eclipse-adore/adore_mission_control.git +[submodule "vendor/libOpenDRIVE/libOpenDRIVE"] + path = vendor/libOpenDRIVE/libOpenDRIVE + url = https://github.com/DLR-TS/libOpenDRIVE.git [submodule "nodes/adore_visualizer"] path = ros2_workspace/src/adore_ros2_nodes/visualizer url = git@github.com:eclipse-adore/adore_visualizer.git diff --git a/ros2_workspace/src/vendor/libOpenDRIVE b/ros2_workspace/src/vendor/libOpenDRIVE index f235e292..2c87711a 160000 --- a/ros2_workspace/src/vendor/libOpenDRIVE +++ b/ros2_workspace/src/vendor/libOpenDRIVE @@ -1 +1 @@ -Subproject commit f235e2928f6d349aa76b3d24cb0181e2f4281da1 +Subproject commit 2c87711a3f9c91f0bc325bd106f68621a80cf999 From 1b22dcf07fe0d9965a4e51446a574383cb25f2e6 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Wed, 1 Jul 2026 13:31:04 +0200 Subject: [PATCH 08/18] added grpc_message_bridge service helper, updated mqtt message brige to support tls Signed-off-by: Andrew Koerner --- adore.env | 50 +++++++++- .../grpc_message_bridge/setup.py | 26 +++++- .../mqtt_message_bridge/.gitignore | 4 + .../config/bridge_config.yaml | 93 ++++++++----------- .../launch/bridge.launch.py | 11 ++- .../mqtt_message_bridge/bridge_node.py | 60 +++++++++--- .../mqtt_message_bridge/utils.py | 59 ++++++++++++ .../mqtt_message_bridge/requirements.pip3 | 1 + .../mqtt_message_bridge/setup.py | 1 - .../start_mqtt_message_bridge.sh | 3 + .../config/bridge_config.yaml | 17 +++- tools/start_grpc_message_bridge.sh | 55 +++++++++++ tools/start_mqtt_bridge.sh | 3 +- 13 files changed, 300 insertions(+), 83 deletions(-) create mode 100644 tools/start_grpc_message_bridge.sh diff --git a/adore.env b/adore.env index 76fb2df3..98710983 100644 --- a/adore.env +++ b/adore.env @@ -87,15 +87,27 @@ ZENOH_BRIDGE_ROUTER=tcp/localhost:7447 # - Set to "true" to launch the bridge node alongside the CLI container. # - Set to "false" or leave unset to disable. # - Default: false -MQTT_BRIDGE_ENABLE=false +MQTT_BRIDGE_ENABLE=true # MQTT_BROKER_HOST: Hostname or IP of the MQTT broker the bridge connects to. # - Default: localhost -MQTT_BROKER_HOST=localhost +#MQTT_BROKER_HOST=localhost # MQTT_BROKER_PORT: Port of the MQTT broker. # - Default: 1883 -MQTT_BROKER_PORT=1883 +#MQTT_BROKER_PORT=1883 + +# MQTT_BRIDGE_CONFIG: Path to the mqtt_message_bridge config file. +# - Single source of truth for the bridge connection (host, port, TLS, certs, +# topic mappings). Relative cert names resolve against MQTT_BRIDGE_CERT_DIR and +# env_file resolves relative to this file. +# - Default: ${SOURCE_DIRECTORY}/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/bridge_config.yaml +MQTT_BRIDGE_CONFIG=${SOURCE_DIRECTORY}/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/bridge_config.yaml + +# MQTT_BRIDGE_CERT_DIR: Directory holding the broker CA and client certs. +# - Relative cert names in MQTT_BRIDGE_CONFIG resolve against this directory. +# - Default: ${SOURCE_DIRECTORY}/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/certs +MQTT_BRIDGE_CERT_DIR=${SOURCE_DIRECTORY}/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/certs # MQTT_LOCAL_BROKER_ENABLE: Start a local Mosquitto broker in Docker on the host. # - Set to "true" to launch eclipse-mosquitto alongside the CLI container. @@ -103,6 +115,38 @@ MQTT_BROKER_PORT=1883 # - Default: false MQTT_LOCAL_BROKER_ENABLE=false +# gRPC Bridge Configuration +# ========================= +# The ADORe CLI optionally runs the grpc_message_bridge ROS node as a host +# process. It is a gRPC CLIENT that dials a remote endpoint over an +# OAuth2-authenticated channel and bridges ROS2 topics to/from it. Logs are +# written to ${LOG_DIRECTORY}/grpc/. +# +# GRPC_BRIDGE_ENABLE: Start the grpc_message_bridge ROS node on the host. +# - Default: false +GRPC_BRIDGE_ENABLE=false + +# GRPC_BRIDGE_CONFIG: Path to the stream/topic mapping config. +GRPC_BRIDGE_CONFIG=${SOURCE_DIRECTORY}/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml + +# GRPC_BRIDGE_PROTO_PATH: Generated python stubs added to PYTHONPATH at runtime. +# - Produced by the package build from the proto submodule. +GRPC_BRIDGE_PROTO_PATH=${SOURCE_DIRECTORY}/ros2_workspace/src/adore_interfaces/grpc_message_bridge/proto/generated + +# GRPC_ENDPOINT: host:port of the remote gRPC service the bridge dials. +#GRPC_ENDPOINT=some_grpc_service.com:443 + +# AUTH_ENDPOINT: OAuth2 token URL. Leave unset for an insecure local channel. +#AUTH_ENDPOINT=https://some_oath_provider.com/auth/token + +# GRPC_BRIDGE_SECRETS: Optional env file holding CLIENT_ID and CLIENT_SECRET. +# - Sourced by the start script if present; keeps OAuth secrets out of adore.env. +GRPC_BRIDGE_SECRETS=${SOURCE_DIRECTORY}/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.secrets.env + +# GRPC_BRIDGE_HOST / GRPC_BRIDGE_PORT: local grpc server bind for server-side streams. +#GRPC_BRIDGE_HOST=0.0.0.0 +#GRPC_BRIDGE_PORT=50051 + # RMW_IMPLEMENTATION: ROS2 middleware to use. # - rmw_fastrtps_cpp: FastDDS (default, no router needed) # - rmw_cyclonedds_cpp: CycloneDDS (no router needed) diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py index 8e4aae85..7f258f0e 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/setup.py @@ -1,11 +1,35 @@ -from setuptools import setup, find_packages +import os +import subprocess + +from setuptools import find_packages, setup +from setuptools.command.build_py import build_py package_name = 'grpc_message_bridge' +here = os.path.abspath(os.path.dirname(__file__)) + + +class GenProto(build_py): + """Generate gRPC stubs from the proto submodule before packaging. + + Runs as part of `colcon build` so a checkout with the proto submodule + initialised produces a working package with no manual `make gen_proto`. + """ + + def run(self): + proto_root = os.environ.get('GRPC_BRIDGE_PROTO_ROOT', os.path.join(here, 'proto')) + proto_out = os.path.join(here, 'proto', 'generated') + subprocess.check_call( + ['make', 'gen_proto', f'PROTO_ROOT={proto_root}', f'PROTO_OUT={proto_out}'], + cwd=here, + ) + super().run() + setup( name = package_name, version = '2.0.0', packages = find_packages(exclude=['test']), + cmdclass = {'build_py': GenProto}, data_files = [ ('share/ament_index/resource_index/packages', ['resource/grpc_message_bridge']), ('share/' + package_name, ['package.xml']), diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore index a14297b5..c37ec263 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore @@ -1,2 +1,6 @@ mqtt_message_bridge/__pycache__ .mqtt_secrets.env +*.crt +*.key +certs/ +!certs/.gitkeep diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml index 46f07801..8d5db801 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml @@ -1,63 +1,46 @@ mqtt: - host: "localhost" + host: localhost port: 1883 - # protocol: mqtt # 'mqtt' (default, MQTTv311) or 'mqttv5' - # transport: tcp # 'tcp' (default) or 'websockets' - # keepalive: 60 - # reconnect_delay: 5 - # reconnect_max_delay: 120 + # host_env: MQTT_HOST + # port_env: MQTT_PORT + keepalive: 60 + transport: tcp # tcp | websockets + protocol: mqtt # mqtt (v3.1.1) | mqttv5 + env_file: .mqtt_secrets.env - # Each parameter above can be overridden by an environment variable. - # Set the *_env key to the name of the env var to read. - # The env var takes precedence over the plain value when both are set. - host_env: "MQTT_HOST" - port_env: "MQTT_PORT" - # protocol_env: "MQTT_PROTOCOL" - # transport_env: "MQTT_TRANSPORT" - # keepalive_env: "MQTT_KEEPALIVE" - # reconnect_delay_env: "MQTT_RECONNECT_DELAY" - # reconnect_max_delay_env: "MQTT_RECONNECT_MAX_DELAY" + auth: + username_env: MQTT_USERNAME + password_env: MQTT_PASSWORD - # Path to a .env file to load before resolving any env vars below. - # Variables already present in the environment take precedence. - # env_file: "/path/to/mqtt.env" + # TLS configuration. + # enabled / ca_certs / certfile / keyfile all support _env variants, + # e.g. enabled_env: MQTT_TLS, ca_certs_env: MQTT_CA_CERT, etc. + tls: + enabled: false + ca_certs: broker.crt + certfile: client.crt + keyfile: client.key - # TLS settings (optional). - # tls: - # ca_certs: "/path/to/ca.crt" - # certfile: "/path/to/client.crt" - # keyfile: "/path/to/client.key" - # insecure: false # Set true to skip certificate verification. + # Set true only to disable hostname verification (testing only). + insecure: false - # Authentication (optional). - auth: - username_env: "MQTT_USERNAME" # Name of the env var holding the username. - password_env: "MQTT_PASSWORD" # Name of the env var holding the password. + reconnect_delay: 1 + reconnect_max_delay: 120 -# Bridge ROS2 DDS topics to MQTT. -ros2_to_mqtt: - - ros_topic: "/ros2_chatter" - mqtt_topic: "ros2/chatter" - msg_type: "std_msgs/msg/String" - # format: cdr # Serialization format: 'cdr' (default) or 'json'. - # # 'json': serializes message fields as a JSON string with - # # 'datatype' metadata appended. - # # The MQTT payload is a UTF-8 JSON string. - # # 'cdr': raw CDR bytes. - # qos_reliability: best_effort # 'best_effort' (default) or 'reliable' - # qos_durability: volatile # 'volatile' (default) or 'transient_local' - # qos_depth: 1 # History depth. Defaults to 1. +ros2_to_mqtt: [] + # - ros_topic: /example/ros_topic + # mqtt_topic: example/mqtt_topic + # msg_type: std_msgs/msg/String + # format: cdr # cdr | json | cdr_json + # qos_depth: 10 + # qos_durability: volatile # volatile | transient_local + # qos_reliability: best_effort # best_effort | reliable -# Bridge MQTT topics to ROS2 DDS. -mqtt_to_ros2: - - mqtt_topic: "mqtt/chatter" - ros_topic: "/mqtt_chatter" - msg_type: "std_msgs/msg/String" - # format: cdr # Serialization format: 'cdr' (default) or 'json'. - # # 'json': expects a JSON string payload with 'datatype' - # # metadata. Deserializes back into msg_type before - # # publishing to DDS. - # # 'cdr': raw CDR bytes. - # qos_reliability: best_effort # 'best_effort' (default) or 'reliable' - # qos_durability: volatile # 'volatile' (default) or 'transient_local' - # qos_depth: 1 # History depth. Defaults to 1. +mqtt_to_ros2: [] + # - mqtt_topic: example/mqtt_topic + # ros_topic: /example/ros_topic + # msg_type: std_msgs/msg/String + # format: cdr + # qos_depth: 10 + # qos_durability: volatile + # qos_reliability: best_effort diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/launch/bridge.launch.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/launch/bridge.launch.py index 5173f16f..d340874b 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/launch/bridge.launch.py +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/launch/bridge.launch.py @@ -1,14 +1,19 @@ from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory import os def generate_launch_description(): - config = os.path.join(get_package_share_directory('mqtt_message_bridge'), 'config', 'bridge_config.yaml') + default_config = os.path.join( + get_package_share_directory('mqtt_message_bridge'), 'config', 'bridge_config.yaml') + config_path = LaunchConfiguration('config_path') return LaunchDescription([ + DeclareLaunchArgument('config_path', default_value=default_config), Node( package='mqtt_message_bridge', executable='bridge_node', - parameters=[{'config_path': config}] - ) + parameters=[{'config_path': config_path}], + ), ]) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py index d3172ea4..dfc44a87 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py @@ -4,13 +4,22 @@ import yaml import paho.mqtt.client as mqtt import rclpy +from ament_index_python.packages import get_package_share_directory from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy -from .utils import load_msg_type, msg_to_bytes, bytes_to_msg, msg_to_json, json_to_msg, msg_to_cdr_json, cdr_json_to_msg +from .utils import load_msg_type, msg_to_bytes, bytes_to_msg, msg_to_json, json_to_msg, msg_to_cdr_json, cdr_json_to_msg, ensure_self_signed_cert _STR_TYPE = 'std_msgs/msg/String' +_PKG_SHARE = get_package_share_directory('mqtt_message_bridge') +_CERT_DIR = os.environ.get('MQTT_BRIDGE_CERT_DIR') or os.path.join(_PKG_SHARE, 'certs') + +def _resolve(path, base=_CERT_DIR): + if not path or os.path.isabs(path): + return path + return os.path.join(base, path) + _PROTOCOL_MAP = { 'mqtt': mqtt.MQTTv311, 'mqttv5': mqtt.MQTTv5, @@ -75,7 +84,10 @@ def __init__(self): self._shutdown_event = threading.Event() self._mqtt_topic_map = {} - self._load_env_file(self.config.get('mqtt', {}).get('env_file')) + env_file = self.config.get('mqtt', {}).get('env_file') + if env_file and not os.path.isabs(env_file): + env_file = os.path.join(os.path.dirname(os.path.abspath(config_path)), env_file) + self._load_env_file(env_file) self._setup_mqtt() self._setup_ros2_to_mqtt() self._setup_mqtt_to_ros2() @@ -100,10 +112,10 @@ def _load_env_file(self, env_file: str | None): self.get_logger().info(f"Loaded env file: {env_file}") @staticmethod - def _env_or(cfg: dict, key: str, default=None): - env_var = cfg.get(f'{key}_env') - if env_var: - val = os.environ.get(env_var) + def _env_or(cfg: dict, key: str, default=None, env_var: str = None): + name = cfg.get(f'{key}_env') or env_var + if name: + val = os.environ.get(name) if val is not None: return val return cfg.get(key, default) @@ -111,8 +123,8 @@ def _env_or(cfg: dict, key: str, default=None): def _setup_mqtt(self): cfg = self.config.get('mqtt', {}) - host = self._env_or(cfg, 'host', 'localhost') - port = int(self._env_or(cfg, 'port', 1883)) + host = self._env_or(cfg, 'host', 'localhost', env_var='MQTT_HOST') + port = int(self._env_or(cfg, 'port', 1883, env_var='MQTT_PORT')) keepalive = int(self._env_or(cfg, 'keepalive', 60)) transport = self._env_or(cfg, 'transport', 'tcp') protocol = _PROTOCOL_MAP.get(self._env_or(cfg, 'protocol', 'mqtt'), mqtt.MQTTv311) @@ -137,8 +149,8 @@ def _configure_auth(self, cfg: dict): if not auth: return - username_env = auth.get('username_env') - password_env = auth.get('password_env') + username_env = auth.get('username_env', 'MQTT_USERNAME') + password_env = auth.get('password_env', 'MQTT_PASSWORD') username = os.environ.get(username_env) if username_env else None password = os.environ.get(password_env) if password_env else None @@ -157,10 +169,32 @@ def _configure_tls(self, cfg: dict): if not tls: return + enabled = self._env_or(tls, 'enabled', False, env_var='MQTT_TLS') + if isinstance(enabled, str): + enabled = enabled.lower() in ('1', 'true', 'yes') + if not enabled: + return + + ca_certs = _resolve(self._env_or(tls, 'ca_certs', env_var='MQTT_CA_CERT')) + certfile = _resolve(self._env_or(tls, 'certfile', env_var='MQTT_CLIENT_CERT')) + keyfile = _resolve(self._env_or(tls, 'keyfile', env_var='MQTT_CLIENT_KEY')) + + if tls.get('generate'): + gen = tls['generate'] + store_dir = _resolve(gen.get('store_dir')) + common_name = gen.get('common_name', 'mqtt_bridge') + validity_days = int(gen.get('validity_days', 3650)) + certfile, keyfile = ensure_self_signed_cert(store_dir, common_name, validity_days) + self.get_logger().info(f'TLS client cert ready: {certfile}') + self.mqtt_client.tls_set( - ca_certs=tls.get('ca_certs'), - certfile=tls.get('certfile'), - keyfile=tls.get('keyfile'), + ca_certs=ca_certs, + certfile=certfile, + keyfile=keyfile, + ) + self.get_logger().info( + f'TLS enabled -- CA: {ca_certs or "system"}' + + (f', cert: {certfile}' if certfile else '') ) if tls.get('insecure', False): self.mqtt_client.tls_insecure_set(True) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/utils.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/utils.py index 6a5bfde5..d40bd4d2 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/utils.py +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/utils.py @@ -1,9 +1,68 @@ import importlib import json +import os +import datetime from rclpy.serialization import serialize_message, deserialize_message from rosidl_runtime_py import message_to_ordereddict, set_message_fields +def ensure_self_signed_cert( + store_dir: str | None = None, + common_name: str = 'mqtt_bridge', + validity_days: int = 3650, +) -> tuple[str, str]: + """Return (certfile, keyfile) paths, generating them if they don't exist. + + Files are written to store_dir (default: ~/.ros/mqtt_bridge/) and are + only created once -- subsequent calls are a no-op as long as the files exist. + """ + from cryptography import x509 + from cryptography.x509.oid import NameOID + from cryptography.hazmat.primitives import hashes, serialization + from cryptography.hazmat.primitives.asymmetric import rsa + + if store_dir is None: + store_dir = os.path.join(os.path.expanduser('~'), '.ros', 'mqtt_bridge') + + os.makedirs(store_dir, mode=0o700, exist_ok=True) + + key_path = os.path.join(store_dir, 'client.key') + cert_path = os.path.join(store_dir, 'client.crt') + + if os.path.exists(key_path) and os.path.exists(cert_path): + return cert_path, key_path + + key = rsa.generate_private_key(public_exponent=65537, key_size=2048) + + with open(key_path, 'wb') as f: + f.write(key.private_bytes( + encoding=serialization.Encoding.PEM, + format=serialization.PrivateFormat.TraditionalOpenSSL, + encryption_algorithm=serialization.NoEncryption(), + )) + os.chmod(key_path, 0o600) + + subject = x509.Name([x509.NameAttribute(NameOID.COMMON_NAME, common_name)]) + now = datetime.datetime.now(datetime.timezone.utc) + cert = ( + x509.CertificateBuilder() + .subject_name(subject) + .issuer_name(subject) + .public_key(key.public_key()) + .serial_number(x509.random_serial_number()) + .not_valid_before(now) + .not_valid_after(now + datetime.timedelta(days=validity_days)) + .add_extension(x509.BasicConstraints(ca=False, path_length=None), critical=True) + .sign(key, hashes.SHA256()) + ) + + with open(cert_path, 'wb') as f: + f.write(cert.public_bytes(serialization.Encoding.PEM)) + + return cert_path, key_path + + + def load_msg_type(msg_type_str: str): try: pkg, interface, name = msg_type_str.split('/') diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/requirements.pip3 b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/requirements.pip3 index 3588ee2e..ca99f721 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/requirements.pip3 +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/requirements.pip3 @@ -1,3 +1,4 @@ paho-mqtt>=2.0.0 pyyaml numpy +cryptography diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/setup.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/setup.py index 78d6fd3f..d6a3b4a9 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/setup.py +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/setup.py @@ -1,5 +1,4 @@ from setuptools import setup -import os package_name = 'mqtt_message_bridge' diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/start_mqtt_message_bridge.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/start_mqtt_message_bridge.sh index 6a044f3a..f2358aa3 100755 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/start_mqtt_message_bridge.sh +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/start_mqtt_message_bridge.sh @@ -1 +1,4 @@ +#!/usr/bin/env bash +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +export MQTT_BRIDGE_CERT_DIR="${MQTT_BRIDGE_CERT_DIR:-$SCRIPT_DIR/certs}" RMW_IMPLEMENTATION=rmw_fastrtps_cpp ros2 launch mqtt_message_bridge bridge.launch.py diff --git a/ros2_workspace/src/adore_interfaces/zenoh_message_bridge/config/bridge_config.yaml b/ros2_workspace/src/adore_interfaces/zenoh_message_bridge/config/bridge_config.yaml index 652fec2f..f8471655 100644 --- a/ros2_workspace/src/adore_interfaces/zenoh_message_bridge/config/bridge_config.yaml +++ b/ros2_workspace/src/adore_interfaces/zenoh_message_bridge/config/bridge_config.yaml @@ -1,4 +1,3 @@ - # To configure zenoh behavior see the `zenoh_bridge_config.json5` @@ -9,18 +8,18 @@ ros_domain_id: 0 # Domain ID embedded in zenoh key expressions. # Must match the ROS_DOMAIN_ID of the rmw_zenoh_cpp peers you are bridging to/from. # Defaults to 0 if unset. -zenoh_bridge_id: 62 +zenoh_bridge_id: 0 # Target RMW distro for zenoh key expression compatibility. # 'humble' uses TypeHashNotSupported (compatible with Humble and older). # 'jazzy' uses the RIHS01 type hash from the local ROS installation. # Defaults to 'humble' if unset. -rmw_target: humble +rmw_target: jazzy # Bridge ROS2 DDS topics to zenoh. # The receiving end must be running rmw_zenoh_cpp with a matching zenoh_bridge_id. ros2_to_zenoh: - - ros_topic: "/ros2_chatter" + - ros_topic: "/fascar" msg_type: "std_msgs/msg/String" # format: cdr # 'cdr' (default): raw CDR bytes. Transport type matches # # msg_type. Consumable by rmw_zenoh_cpp natively. @@ -36,10 +35,13 @@ ros2_to_zenoh: # qos_durability: volatile # 'volatile' (default) or 'transient_local' # qos_depth: 1 # History depth. Defaults to 1. + - ros_topic: "/remote_operation/backend_to_vehicle" + msg_type: "remote_operations_msgs/msg/RemoteOperationsBackendToVehicle" + # Bridge zenoh topics to ROS2 DDS. # The publishing end must be running rmw_zenoh_cpp with a matching zenoh_bridge_id. zenoh_to_ros2: - - ros_topic: "/zenoh_chatter" + - ros_topic: "/ros2_chatter" msg_type: "std_msgs/msg/String" # format: cdr # 'cdr' (default): raw CDR bytes. Subscribes on the # # msg_type zenoh key. @@ -54,3 +56,8 @@ zenoh_to_ros2: # qos_reliability: best_effort # 'best_effort' (default) or 'reliable' # qos_durability: volatile # 'volatile' (default) or 'transient_local' # qos_depth: 1 # History depth. Defaults to 1. + + + - ros_topic: "/remote_operation/vehicle_to_backend" + msg_type: "remote_operations_msgs/msg/RemoteOperationsVehicleToBackend" +``` diff --git a/tools/start_grpc_message_bridge.sh b/tools/start_grpc_message_bridge.sh new file mode 100644 index 00000000..5c7f2f01 --- /dev/null +++ b/tools/start_grpc_message_bridge.sh @@ -0,0 +1,55 @@ +#!/usr/bin/env bash +SCRIPT_DIRECTORY="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" +export SOURCE_DIRECTORY="$(realpath "${SCRIPT_DIRECTORY}/..")" + +source "${SOURCE_DIRECTORY}/adore.env" +source "/opt/ros/${ROS_DISTRO}/setup.bash" 2>/dev/null || true + +ROS2_WORKSPACE_DIRECTORY="${SOURCE_DIRECTORY}/ros2_workspace" +if [ -f "${ROS2_WORKSPACE_DIRECTORY}/install/local_setup.bash" ]; then + source "${ROS2_WORKSPACE_DIRECTORY}/install/local_setup.bash" +fi + +LOG_DIR="${SOURCE_DIRECTORY}/.log/grpc" +PIDFILE="${LOG_DIR}/grpc_bridge.pid" +LOGFILE="${LOG_DIR}/grpc_bridge.log" + +mkdir -p "${LOG_DIR}" + +if [ "${GRPC_BRIDGE_ENABLE:-false}" != "true" ]; then + exit 0 +fi + +if [ -f "${PIDFILE}" ] && kill -0 "$(cat "${PIDFILE}")" 2>/dev/null; then + echo "✓ gRPC bridge already running (pid $(cat "${PIDFILE}"))" + exit 0 +fi + +# OAuth client_id/client_secret are kept out of adore.env and sourced here. +if [ -f "${GRPC_BRIDGE_SECRETS:-}" ]; then + set -a + source "${GRPC_BRIDGE_SECRETS}" + set +a +fi + +# proto_registry imports the generated stubs at module load, so the generated +# tree produced by the package build must be importable. +if [ -n "${GRPC_BRIDGE_PROTO_PATH:-}" ]; then + export PYTHONPATH="${GRPC_BRIDGE_PROTO_PATH}:${PYTHONPATH}" +fi + +if [ -z "${GRPC_ENDPOINT:-}" ]; then + echo "WARNING: GRPC_ENDPOINT is unset; the bridge has no remote to dial" >&2 +fi + +export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}" + +echo "Starting grpc_message_bridge -> ${LOGFILE}" +ros2 launch grpc_message_bridge bridge.launch.py \ + config_path:="${GRPC_BRIDGE_CONFIG}" \ + grpc_host:="${GRPC_BRIDGE_HOST:-0.0.0.0}" \ + grpc_port:="${GRPC_BRIDGE_PORT:-50051}" \ + >> "${LOGFILE}" 2>&1 & +BRIDGE_PID=$! +echo $BRIDGE_PID > "${PIDFILE}" +echo " pid ${BRIDGE_PID}" diff --git a/tools/start_mqtt_bridge.sh b/tools/start_mqtt_bridge.sh index d63ef218..9a9503eb 100644 --- a/tools/start_mqtt_bridge.sh +++ b/tools/start_mqtt_bridge.sh @@ -28,8 +28,7 @@ fi echo "Starting mqtt_message_bridge -> ${LOGFILE}" export RMW_IMPLEMENTATION=rmw_fastrtps_cpp ros2 launch mqtt_message_bridge bridge.launch.py \ - mqtt_broker:="${MQTT_BROKER_HOST:-localhost}" \ - mqtt_port:="${MQTT_BROKER_PORT:-1883}" \ + config_path:="${MQTT_BRIDGE_CONFIG}" \ >> "${LOGFILE}" 2>&1 & BRIDGE_PID=$! echo $BRIDGE_PID > "${PIDFILE}" From 84da71505a3ef033fa90ff7fd3158640723d2b47 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Wed, 1 Jul 2026 14:11:01 +0200 Subject: [PATCH 09/18] removed duplicate entries from gitmodules Signed-off-by: Andrew Koerner --- .gitmodules | 4 ---- 1 file changed, 4 deletions(-) diff --git a/.gitmodules b/.gitmodules index 866844f5..3dc1edf6 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,9 +7,6 @@ [submodule "mission_control"] path = ros2_workspace/src/adore_ros2_nodes/mission_control url = git@github.com:eclipse-adore/adore_mission_control.git -[submodule "vendor/libOpenDRIVE/libOpenDRIVE"] - path = vendor/libOpenDRIVE/libOpenDRIVE - url = https://github.com/DLR-TS/libOpenDRIVE.git [submodule "nodes/adore_visualizer"] path = ros2_workspace/src/adore_ros2_nodes/visualizer url = git@github.com:eclipse-adore/adore_visualizer.git @@ -86,7 +83,6 @@ [submodule "vendor/mathematics_toolbox"] path = vendor/mathematics_toolbox url = git@github.com:dlr-ts/mathematics_toolbox.git - [submodule "vendor/ros2_observer"] path = vendor/ros2_observer url = git@github.com:dlr-ts/ros2_observer.git From 882d2a7819c94a6f108f842ead383fbb70c58f8f Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Tue, 7 Jul 2026 11:33:40 +0200 Subject: [PATCH 10/18] fixed libopendrive path in .gitmodules Signed-off-by: Andrew Koerner --- .gitmodules | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.gitmodules b/.gitmodules index 3dc1edf6..ad1776aa 100644 --- a/.gitmodules +++ b/.gitmodules @@ -89,8 +89,8 @@ [submodule "tools/lichtblick/lichtblick"] path = tools/lichtblick/lichtblick url = https://github.com/Lichtblick-Suite/lichtblick.git -[submodule "vendor/libOpenDRIVE/libOpenDRIVE"] - path = vendor/libOpenDRIVE/libOpenDRIVE +[submodule "ros2_workspace/src/vendor/libOpenDRIVE"] + path = ros2_workspace/src/vendor/libOpenDRIVE url = https://github.com/DLR-TS/libopendrive.git [submodule "ros2_workspace/src/vendor/osqp-eigen"] path = ros2_workspace/src/vendor/osqp_eigen From e97972b88c1cff477f4a85654608d0d8e0ec3a7e Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Mon, 27 Jul 2026 12:25:35 +0200 Subject: [PATCH 11/18] fixed semantic error in grpc bridge parsing Signed-off-by: Andrew Koerner --- .../grpc_message_bridge/.gitignore | 1 + .../grpc_message_bridge/bridge_node.py | 27 +++- .../grpc_message_bridge/codec.py | 55 +++---- .../start_grpc_message_bridge.sh | 14 ++ .../tools/test_bridge_path.py | 140 ++++++++++++++++++ 5 files changed, 204 insertions(+), 33 deletions(-) create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_bridge_path.py diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore index 8254e732..5718aa8d 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/.gitignore @@ -1,2 +1,3 @@ .secrets.env proto/generated +proto/ diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py index 3887104d..1b5bb1e4 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py @@ -1,7 +1,9 @@ +import logging import os import queue import threading import time +import traceback import grpc import rclpy @@ -63,11 +65,15 @@ def __init__(self): self.ros_subs: list = [] self.ros_publish_queue = queue.Queue() self.stream_send_queues: dict = {} # stream.key -> Queue[proto_msg] + self._last_sent: dict = {} # stream.key -> {oneof_field: proto_msg} self.shutdown_event = threading.Event() self._grpc_server = None self._grpc_channels: dict = {} + from . import codec as _codec + self.get_logger().info(f'codec module: {_codec.__file__}') + self._setup_publishers() self._setup_grpc_server() self._setup_server_streams() @@ -91,7 +97,7 @@ def _setup_publishers(self): wire_type = wire_ros_type(fm.ros_msg_type, fm.format) ros_type = load_ros_type(wire_type) self.ros_pubs[fm.ros_topic] = self.create_publisher( - ros_type, fm.ros_topic, _qos({}, 'best_effort')) + ros_type, fm.ros_topic, _qos(self.config.get('qos', {}), 'reliable')) self.get_logger().info(f'Publisher: {fm.ros_topic}') # ------------------------------------------------------------------ @@ -140,13 +146,16 @@ def _setup_client_streams(self): self._setup_send_subscriptions(stream) send_queue = self.stream_send_queues.setdefault(stream.key, queue.Queue()) - def _sender(sq=send_queue, shutdown=self.shutdown_event): - """Yield queued messages, keeping the stream open until shutdown.""" + def _sender(sq=send_queue, shutdown=self.shutdown_event, s=stream): + """Replay the last message per oneof field, then stream live ones until shutdown.""" + for msg in list(self._last_sent.get(s.key, {}).values()): + yield msg while not shutdown.is_set(): try: msg = sq.get(timeout=0.5) if msg is None: return + self._last_sent.setdefault(s.key, {})[active_oneof_field(msg)] = msg yield msg except queue.Empty: continue @@ -174,6 +183,9 @@ def _worker(addr=remote_addr, s=stream, sq=send_queue, sender=_sender): self.get_logger().warn( f'[{s.key}] {e.code().name}: {e.details()} -- reconnecting in 2s') time.sleep(2) + except Exception: + self.get_logger().error(f'[{s.key}] worker: {traceback.format_exc()}') + time.sleep(2) threading.Thread(target=_worker, daemon=True).start() self.get_logger().info(f'Client stream: {key} -> {remote_addr}') @@ -196,8 +208,9 @@ def _setup_send_subscriptions(self, stream: StreamDef): if any(s.topic_name == fm.ros_topic for s in self.ros_subs): continue # already subscribed - ros_type = load_ros_type(fm.ros_msg_type) - serialize = make_ros_serializer(fm.ros_msg_type, fm.format) + wire_type = wire_ros_type(fm.ros_msg_type, fm.format) + ros_type = load_ros_type(wire_type) + serialize = make_ros_serializer(wire_type, fm.format) send_queue = self.stream_send_queues.setdefault(stream.key, queue.Queue()) def cb(ros_msg, f=fm, cls=stream.send_msg_cls, sq=send_queue, ser=serialize): @@ -209,7 +222,8 @@ def cb(ros_msg, f=fm, cls=stream.send_msg_cls, sq=send_queue, ser=serialize): self.get_logger().error( f'[{stream.key}] pack {f.field_name}: {e}') - sub = self.create_subscription(ros_type, fm.ros_topic, cb, _qos({}, 'reliable')) + sub = self.create_subscription(ros_type, fm.ros_topic, cb, + _qos(self.config.get('qos', {}), 'reliable')) self.ros_subs.append(sub) self.get_logger().info(f'Send sub: {fm.ros_topic} -> {stream.key}.{fm.field_name}') @@ -264,6 +278,7 @@ def shutdown(self): def main(args=None): + logging.basicConfig(level=logging.INFO, format='[auth] %(levelname)s %(message)s') rclpy.init(args=args) node = ROS2GrpcBridge() executor = MultiThreadedExecutor() diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/codec.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/codec.py index 3bf30d11..a684b992 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/codec.py +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/codec.py @@ -7,10 +7,10 @@ import json import sys +import uuid from typing import Any, Callable, Optional from rclpy.serialization import serialize_message, deserialize_message -from rosidl_runtime_py import message_to_ordereddict, set_message_fields _STR_TYPE = 'std_msgs/msg/String' @@ -35,20 +35,14 @@ def wire_ros_type(ros_type: str, fmt: str) -> str: # --------------------------------------------------------------------------- def make_ros_serializer(ros_type: str, fmt: str) -> Callable: + """For json/cdr_json the wire type is std_msgs/String and `data` carries the proto JSON.""" if fmt == 'json': - def _to_json(msg, rt=ros_type) -> bytes: - obj = message_to_ordereddict(msg) - obj['datatype'] = rt - return json.dumps(obj).encode() - return _to_json + return lambda msg: msg.data.encode() if fmt == 'cdr_json': - def _to_cdr_json(msg, rt=ros_type) -> bytes: + def _to_cdr_json(msg) -> bytes: from std_msgs.msg import String - obj = message_to_ordereddict(msg) - obj['datatype'] = rt - wrapper = String(data=json.dumps(obj)) - return serialize_message(wrapper) + return serialize_message(String(data=msg.data)) return _to_cdr_json return serialize_message @@ -56,25 +50,15 @@ def _to_cdr_json(msg, rt=ros_type) -> bytes: def make_ros_deserializer(msg_type, fmt: str) -> Callable: if fmt == 'json': - def _from_json(data: bytes, mt=msg_type): - obj = json.loads(data.decode()) - obj.pop('datatype', None) - obj.pop('topic', None) - msg = mt() - set_message_fields(msg, obj) - return msg + def _from_json(data: bytes): + from std_msgs.msg import String + return String(data=data.decode()) return _from_json if fmt == 'cdr_json': - def _from_cdr_json(data: bytes, mt=msg_type): + def _from_cdr_json(data: bytes): from std_msgs.msg import String - wrapper = deserialize_message(data, String) - obj = json.loads(wrapper.data) - obj.pop('datatype', None) - obj.pop('topic', None) - msg = mt() - set_message_fields(msg, obj) - return msg + return deserialize_message(data, String) return _from_cdr_json return lambda data, mt=msg_type: deserialize_message(data, mt) @@ -114,6 +98,21 @@ def proto_field_to_bytes(proto_msg: Any, field_name: str, fmt: str) -> Optional[ return value.SerializeToString() +def _populate_metadata(instance: Any) -> None: + """Fill the non-oneof metadata envelope the gateway requires alongside every payload.""" + desc = type(instance).DESCRIPTOR.fields_by_name.get('metadata') + if desc is None or desc.type != desc.TYPE_MESSAGE: + return + + md = getattr(instance, 'metadata') + fields = desc.message_type.fields_by_name + + if 'message_id' in fields and not md.message_id: + md.message_id = str(uuid.uuid4()) + if 'timestamp' in fields: + md.timestamp.GetCurrentTime() + + def proto_field_set(proto_msg_cls, field_name: str, payload: bytes, fmt: str) -> Any: """ Create a new proto message of proto_msg_cls with field_name set from payload bytes. @@ -127,6 +126,7 @@ def proto_field_set(proto_msg_cls, field_name: str, payload: bytes, fmt: str) -> # Scalar bytes/string -- set directly. if field_desc.type in (field_desc.TYPE_BYTES, field_desc.TYPE_STRING): setattr(instance, field_name, payload) + _populate_metadata(instance) return instance # Nested message -- find its class and populate. @@ -140,6 +140,7 @@ def proto_field_set(proto_msg_cls, field_name: str, payload: bytes, fmt: str) -> else: getattr(instance, field_name).MergeFrom(nested_cls.FromString(payload)) + _populate_metadata(instance) return instance @@ -163,7 +164,7 @@ def active_oneof_field(proto_msg: Any) -> str: def _proto_to_dict(msg) -> dict: """Shallow proto-message-to-dict, suitable for JSON serialization.""" from google.protobuf.json_format import MessageToDict - return MessageToDict(msg, preserving_proto_field_name=True, including_default_value_fields=False) + return MessageToDict(msg, preserving_proto_field_name=True) def _dict_to_proto(proto_cls, d: dict, target=None): diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_grpc_message_bridge.sh b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_grpc_message_bridge.sh index af81784a..23f3d564 100755 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_grpc_message_bridge.sh +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/start_grpc_message_bridge.sh @@ -1,10 +1,24 @@ #!/usr/bin/env bash set -euo pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +if [[ -f "$SCRIPT_DIR/.secrets.env" ]]; then + set -a + source "$SCRIPT_DIR/.secrets.env" + set +a +fi + +: "${GRPC_ENDPOINT:?GRPC_ENDPOINT not set, source .secrets.env}" + +export PYTHONPATH="${PYTHONPATH:-}:$SCRIPT_DIR/proto/generated" + GRPC_HOST="${GRPC_HOST:-0.0.0.0}" GRPC_PORT="${GRPC_PORT:-50051}" BRIDGE_CONFIG_PATH="${BRIDGE_CONFIG_PATH:-$(dirname "$0")/config/bridge_config.yaml}" SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +export PYTHONPATH="$PYTHONPATH:$(pwd)/proto/generated" + # Set USE_LOCAL_GRPC_SERVER=1 to start a local gRPC server before the bridge node. if [[ "${USE_LOCAL_GRPC_SERVER:-}" == "1" ]]; then diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_bridge_path.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_bridge_path.py new file mode 100644 index 00000000..7a629d7b --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_bridge_path.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +""" +Drives the bridge's own codec + proto_registry + auth through the full +subscribe path without a ROS runtime, so gRPC-side and ROS-side failures +can be told apart. + + set -a && source .secrets.env && set +a + PYTHONPATH=proto/generated:. python3 tools/test_bridge_path.py +""" + +import json +import logging +import os +import queue +import sys +import threading +import time + +_ROOT = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +sys.path.insert(0, os.path.join(_ROOT, 'proto', 'generated')) +sys.path.insert(0, _ROOT) + +logging.basicConfig(level=logging.INFO, format='[%(name)s] %(levelname)s %(message)s') + +import grpc +from std_msgs.msg import String + +from grpc_message_bridge.auth import make_channel +from grpc_message_bridge.codec import ( + active_oneof_field, + load_ros_type, + make_ros_deserializer, + make_ros_serializer, + proto_field_set, + proto_field_to_bytes, + wire_ros_type, +) +from grpc_message_bridge.proto_registry import STREAMS + +STREAM_KEY = 'client.ClientService.ClientStream' +SUB_TOPIC = '/supervision/subscription' +DURATION_S = 15 + + +def main() -> int: + stream = STREAMS[STREAM_KEY] + + import grpc_message_bridge.codec as _codec + print(f'codec {_codec.__file__}') + if not _codec.__file__.startswith(_ROOT): + print(f'WARNING imported from outside {_ROOT}, a stale build copy is shadowing src') + + print(f'endpoint {os.environ.get("GRPC_ENDPOINT", "")}') + print(f'auth {os.environ.get("AUTH_ENDPOINT", "")}') + if not os.environ.get('AUTH_ENDPOINT'): + print('AUTH_ENDPOINT unset, make_channel will fall back to insecure') + + send_fm = next(fm for fm in stream.send_fields if fm.ros_topic == SUB_TOPIC) + wire_type = wire_ros_type(send_fm.ros_msg_type, send_fm.format) + serialize = make_ros_serializer(wire_type, send_fm.format) + + ros_msg = String(data=json.dumps({'type': 'TELEMETRY', 'vehicleIds': []})) + payload = serialize(ros_msg) + print(f'\npayload {payload!r}') + + sub_msg = proto_field_set(stream.send_msg_cls, send_fm.field_name, payload, send_fm.format) + print('packed ' + (str(sub_msg).strip().replace('\n', '\n ') or '')) + if active_oneof_field(sub_msg) != send_fm.field_name: + print(f'FAIL oneof is {active_oneof_field(sub_msg)!r}, expected {send_fm.field_name!r}') + return 1 + + send_q = queue.Queue() + send_q.put(sub_msg) + stop = threading.Event() + + def sender(): + while not stop.is_set(): + try: + yield send_q.get(timeout=0.5) + except queue.Empty: + continue + + channel = make_channel() + stub = stream.stub_cls(channel) + rpc = getattr(stub, stream.rpc) + + counts = {} + published = 0 + started = time.monotonic() + print(f'\nstreaming for {DURATION_S}s\n') + + try: + for server_msg in rpc(sender(), timeout=DURATION_S): + field = active_oneof_field(server_msg) + counts[field] = counts.get(field, 0) + 1 + + fm = stream.recv_field_map.get(field) + if fm is None: + print(f' {field:<16} no recv_field_map entry, dropped') + else: + data = proto_field_to_bytes(server_msg, field, fm.format) + if data is None: + print(f' {field:<16} proto_field_to_bytes returned None') + else: + ros_type = load_ros_type(wire_ros_type(fm.ros_msg_type, fm.format)) + try: + out = make_ros_deserializer(ros_type, fm.format)(data) + if published < 3: + print(f' {field:<16} -> {fm.ros_topic} {out}') + published += 1 + except Exception as e: + print(f' {field:<16} deser failed: {type(e).__name__}: {e}') + + except grpc.RpcError as e: + if e.code() is not grpc.StatusCode.DEADLINE_EXCEEDED: + print(f'\nstream error: {e.code().name}: {e.details()}') + return 1 + except Exception as e: + print(f'\nstream error: {type(e).__name__}: {e}') + return 1 + finally: + stop.set() + channel.close() + + print(f'\nelapsed {time.monotonic() - started:.1f}s') + print(f'received {counts or "nothing"}') + print(f'publishable {published}') + + if not counts: + print('\ngRPC side is the problem, the gateway sent nothing on this stream') + return 1 + if not published: + print('\ncodec is the problem, messages arrived but none survived conversion') + return 1 + print('\ngRPC and codec both fine, remaining fault is in the ROS publish path') + return 0 + + +if __name__ == '__main__': + sys.exit(main()) From 9f2bf54b9c0ae4e7a1a0a3d9aa01e43396391b83 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Wed, 29 Jul 2026 10:17:29 +0200 Subject: [PATCH 12/18] added mqtt publish test script Signed-off-by: Andrew Koerner --- .../config/bridge_config_diamos.yaml | 70 ++++++ .../scripts/publish_vehicle_state.py | 210 ++++++++++++++++++ 2 files changed, 280 insertions(+) create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config_diamos.yaml create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/publish_vehicle_state.py diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config_diamos.yaml b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config_diamos.yaml new file mode 100644 index 00000000..ec6401b4 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config_diamos.yaml @@ -0,0 +1,70 @@ +mqtt: + host: localhost + port: 1883 + # host_env: MQTT_HOST + # port_env: MQTT_PORT + keepalive: 60 + transport: tcp # tcp | websockets + protocol: mqtt # mqtt (v3.1.1) | mqttv5 + env_file: ../.mqtt_secrets.env + + auth: + username_env: MQTT_USERNAME + password_env: MQTT_PASSWORD + + # TLS configuration. + # enabled / ca_certs / certfile / keyfile all support _env variants, + # e.g. enabled_env: MQTT_TLS, ca_certs_env: MQTT_CA_CERT, etc. + # Relative paths resolve against MQTT_BRIDGE_CERT_DIR. + # The broker enforces mTLS, so certfile and keyfile are both required. + tls: + enabled: true + ca_certs: broker.crt + certfile: client.crt + keyfile: client.key + + # Set true only to disable hostname verification (testing only). + insecure: false + + reconnect_delay: 1 + reconnect_max_delay: 120 + +# Northbound: ROS 2 -> MQTT -> Supervision ingest -> gRPC ClientStream. +# `raw` publishes msg.data verbatim, so the ROS String must already hold the +# gateway JSON. Anything else nests it inside a bridge envelope. +# The imoger_dlr ACL grants od_imoger/# only. +ros2_to_mqtt: + - ros_topic: /supervision/telemetry + mqtt_topic: od_imoger/adore/MV-001/telemetry + msg_type: std_msgs/msg/String + format: raw + qos_depth: 1 + qos_reliability: best_effort + qos_durability: volatile + + - ros_topic: /supervision/notifications + mqtt_topic: od_imoger/adore/MV-001/notifications + msg_type: std_msgs/msg/String + format: raw + qos_depth: 20 + qos_reliability: reliable + qos_durability: volatile + +# Loopback for local verification: whatever lands on the northbound topics comes +# back into ROS so `ros2 topic echo` can inspect the exact bytes on the wire. +mqtt_to_ros2: + - mqtt_topic: od_imoger/adore/MV-001/telemetry + ros_topic: /supervision/telemetry_echo + msg_type: std_msgs/msg/String + format: raw + qos_depth: 1 + qos_reliability: reliable + qos_durability: transient_local + + - mqtt_topic: od_imoger/adore/MV-001/notifications + ros_topic: /supervision/notifications_echo + msg_type: std_msgs/msg/String + format: raw + qos_depth: 20 + qos_reliability: reliable + qos_durability: transient_local diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/publish_vehicle_state.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/publish_vehicle_state.py new file mode 100644 index 00000000..1adeae09 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/publish_vehicle_state.py @@ -0,0 +1,210 @@ +#!/usr/bin/env python3 +"""Take one message off /ego_vehicle/vehicle_state_dynamic and republish it as a +Supervision Gateway VehicleTelemetryUpdate. + + python3 scripts/publish_vehicle_state.py +""" +import json +import math +import os +import re +import time + +import rclpy +from adore_ros2_msgs.msg import TrafficParticipantSet, VehicleStateDynamic +from rclpy.qos import QoSProfile, DurabilityPolicy, HistoryPolicy, ReliabilityPolicy +from std_msgs.msg import String + +STATE_TOPIC = '/ego_vehicle/vehicle_state_dynamic' +PARTICIPANT_TOPIC = '/ego_vehicle/traffic_participants' +TELEMETRY_TOPIC = '/supervision/telemetry' +TIMEOUT = 10.0 + +VEHICLE_ID = 'MV-001' +UTM_ZONE = 32 +UTM_NORTHERN = True + +# No source in VehicleStateDynamic. +STATE = 'NOMINAL_DRIVING' +BATTERY = 100.0 +PASSENGERS = 0 + +_A_AXIS = 6378137.0 +_F = 1.0 / 298.257223563 +_K0 = 0.9996 +_FALSE_EASTING = 500000.0 +_FALSE_NORTHING = 10000000.0 + +_n = _F / (2.0 - _F) +_A = (_A_AXIS / (1.0 + _n)) * (1 + _n**2 / 4 + _n**4 / 64 + _n**6 / 256) + +_BETA = ( + _n / 2 - 2 * _n**2 / 3 + 37 * _n**3 / 96 - _n**4 / 360 - 81 * _n**5 / 512 + 96199 * _n**6 / 604800, + _n**2 / 48 + _n**3 / 15 - 437 * _n**4 / 1440 + 46 * _n**5 / 105 - 1118711 * _n**6 / 3870720, + 17 * _n**3 / 480 - 37 * _n**4 / 840 - 209 * _n**5 / 4480 + 5569 * _n**6 / 90720, + 4397 * _n**4 / 161280 - 11 * _n**5 / 504 - 830251 * _n**6 / 7257600, + 4583 * _n**5 / 161280 - 108847 * _n**6 / 3991680, + 20648693 * _n**6 / 638668800, +) +_DELTA = ( + 2 * _n - 2 * _n**2 / 3 - 2 * _n**3 + 116 * _n**4 / 45 + 26 * _n**5 / 45 - 2854 * _n**6 / 675, + 7 * _n**2 / 3 - 8 * _n**3 / 5 - 227 * _n**4 / 45 + 2704 * _n**5 / 315 + 2323 * _n**6 / 945, + 56 * _n**3 / 15 - 136 * _n**4 / 35 - 1262 * _n**5 / 105 + 73814 * _n**6 / 2835, + 4279 * _n**4 / 630 - 332 * _n**5 / 35 - 399572 * _n**6 / 14175, + 4174 * _n**5 / 315 - 144838 * _n**6 / 6237, + 601676 * _n**6 / 22275, +) + + +def utm_to_latlon(easting, northing, zone, northern): + """Inverse transverse Mercator, Krueger series. Avoids a PROJ data dependency.""" + xi = (northing if northern else northing - _FALSE_NORTHING) / (_K0 * _A) + eta = (easting - _FALSE_EASTING) / (_K0 * _A) + + xi_p = xi - sum(b * math.sin(2 * j * xi) * math.cosh(2 * j * eta) + for j, b in enumerate(_BETA, 1)) + eta_p = eta - sum(b * math.cos(2 * j * xi) * math.sinh(2 * j * eta) + for j, b in enumerate(_BETA, 1)) + + chi = math.asin(math.sin(xi_p) / math.cosh(eta_p)) + phi = chi + sum(d * math.sin(2 * j * chi) for j, d in enumerate(_DELTA, 1)) + lam = math.radians(zone * 6 - 183) + math.atan2(math.sinh(eta_p), math.cos(xi_p)) + + return math.degrees(phi), (math.degrees(lam) + 180.0) % 360.0 - 180.0 + + +def utm_frame(frame_id): + """'UTM32U' -> (32, True). Falls back to the configured zone if unrecognised.""" + m = re.match(r'^UTM(\d{1,2})([C-HJ-NP-X])?$', frame_id or '', re.IGNORECASE) + if not m: + return UTM_ZONE, UTM_NORTHERN + return int(m.group(1)), (m.group(2) or 'N').upper() >= 'N' + + +def speed(state): + return math.hypot(state.vx, state.vy) + + +def participant_to_obstacle(participant, ego): + motion = participant.motion_state + body = participant.physical_parameters + dx, dy = motion.x - ego.x, motion.y - ego.y + c, s = math.cos(-ego.yaw_angle), math.sin(-ego.yaw_angle) + return { + 'position': {'x': c * dx - s * dy, 'y': s * dx + c * dy}, + 'heading': motion.yaw_angle, + 'dimensions': { + 'height': body.body_height, + 'width': body.body_width, + 'length': body.body_length, + }, + 'velocity': speed(motion), + } + + +def build_telemetry(ego, participants): + zone, northern = utm_frame(ego.header.frame_id) + lat, lon = utm_to_latlon(ego.x, ego.y, zone, northern) + + return { + 'telemetry': { + 'vehicleId': VEHICLE_ID, + 'telemetry': { + 'state': STATE, + 'position': {'lat': lat, 'lon': lon}, + 'heading': ego.yaw_angle, + 'velocity': speed(ego), + 'battery': BATTERY, + 'obstacles': [participant_to_obstacle(d.participant_data, ego) + for d in participants], + 'acceleration': ego.ax, + 'passengers': PASSENGERS, + }, + 'isConnected': True, + } + } + + +def _no_message_reason(node): + infos = node.get_publishers_info_by_topic(STATE_TOPIC) + if not infos: + rmw = os.environ.get('RMW_IMPLEMENTATION', 'rmw_fastrtps_cpp (default)') + return (f'nothing is publishing {STATE_TOPIC}. Check the topic name, and that ' + f'the publisher uses the same middleware (RMW_IMPLEMENTATION={rmw}).') + expected = 'adore_ros2_msgs/msg/VehicleStateDynamic' + wrong = [i.topic_type for i in infos if i.topic_type != expected] + if wrong: + return (f'{STATE_TOPIC} carries {wrong[0]}, not {expected}. ' + f'A subscription with the wrong type never matches.') + endpoints = ', '.join( + f'{i.node_name} [{i.qos_profile.reliability.name}/{i.qos_profile.durability.name}]' + for i in infos + ) + return f'{len(infos)} publisher(s) on {STATE_TOPIC} but nothing arrived in {TIMEOUT:g}s: {endpoints}' + + +def _sub_qos(): + """Best effort + volatile so the subscription matches any publisher QoS.""" + return QoSProfile( + depth=1, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.BEST_EFFORT, + durability=DurabilityPolicy.VOLATILE, + ) + + +def _pub_qos(): + """Reliable so the bridge's best-effort subscription still matches, but the + single message is not dropped on the way out.""" + return QoSProfile( + depth=1, + history=HistoryPolicy.KEEP_LAST, + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.VOLATILE, + ) + + +def capture_and_publish(node): + ego, participants = [], [] + node.create_subscription(VehicleStateDynamic, STATE_TOPIC, ego.append, _sub_qos()) + node.create_subscription(TrafficParticipantSet, PARTICIPANT_TOPIC, + lambda m: participants.append(m.data), _sub_qos()) + pub = node.create_publisher(String, TELEMETRY_TOPIC, _pub_qos()) + + deadline = time.monotonic() + TIMEOUT + while not ego and time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if not ego: + raise SystemExit(_no_message_reason(node)) + rclpy.spin_once(node, timeout_sec=0.5) + + payload = json.dumps(build_telemetry(ego[0], participants[-1] if participants else [])) + + deadline = time.monotonic() + 5.0 + while node.count_subscribers(TELEMETRY_TOPIC) == 0 and time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + if node.count_subscribers(TELEMETRY_TOPIC) == 0: + raise SystemExit(f'no subscriber on {TELEMETRY_TOPIC}; is the bridge running?') + + pub.publish(String(data=payload)) + for _ in range(5): + rclpy.spin_once(node, timeout_sec=0.1) + return payload + + +def main(): + rclpy.init() + node = rclpy.create_node('publish_vehicle_state') + try: + payload = capture_and_publish(node) + except KeyboardInterrupt: + raise SystemExit('interrupted') + finally: + node.destroy_node() + rclpy.try_shutdown() + + print(f'published {len(payload)} bytes to {TELEMETRY_TOPIC}') + + +if __name__ == '__main__': + main() From 569749cdaa15b593b4f4857235bae1587ab9bcf7 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Wed, 5 Aug 2026 13:08:02 +0200 Subject: [PATCH 13/18] updated mqtt_message bridge and adore_cli Signed-off-by: Andrew Koerner --- adore_cli | 2 +- .../hardware_monitor/requirements.pip3 | 1 + .../config/bridge_config.yaml | 74 ++++++++++++++----- .../config/bridge_config_diamos.yaml | 70 ------------------ .../mqtt_message_bridge/bridge_node.py | 68 +++++++++++++++-- .../mqtt_message_bridge/utils.py | 12 +++ 6 files changed, 131 insertions(+), 96 deletions(-) delete mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config_diamos.yaml diff --git a/adore_cli b/adore_cli index 611a92d3..79f54848 160000 --- a/adore_cli +++ b/adore_cli @@ -1 +1 @@ -Subproject commit 611a92d375cd782f177cfddb1f51e259e5e3b097 +Subproject commit 79f54848eb9ba2a65ec59fab65b4eac106008e69 diff --git a/ros2_workspace/src/adore_interfaces/hardware_monitor/requirements.pip3 b/ros2_workspace/src/adore_interfaces/hardware_monitor/requirements.pip3 index 2af14583..689a3eab 100644 --- a/ros2_workspace/src/adore_interfaces/hardware_monitor/requirements.pip3 +++ b/ros2_workspace/src/adore_interfaces/hardware_monitor/requirements.pip3 @@ -1 +1,2 @@ psutil>=5.9.0 +setuptools<80 diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml index 8d5db801..4c63c424 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml @@ -15,8 +15,10 @@ mqtt: # TLS configuration. # enabled / ca_certs / certfile / keyfile all support _env variants, # e.g. enabled_env: MQTT_TLS, ca_certs_env: MQTT_CA_CERT, etc. + # Relative paths resolve against MQTT_BRIDGE_CERT_DIR. + # The broker enforces mTLS, so certfile and keyfile are both required. tls: - enabled: false + enabled: true ca_certs: broker.crt certfile: client.crt keyfile: client.key @@ -27,20 +29,56 @@ mqtt: reconnect_delay: 1 reconnect_max_delay: 120 -ros2_to_mqtt: [] - # - ros_topic: /example/ros_topic - # mqtt_topic: example/mqtt_topic - # msg_type: std_msgs/msg/String - # format: cdr # cdr | json | cdr_json - # qos_depth: 10 - # qos_durability: volatile # volatile | transient_local - # qos_reliability: best_effort # best_effort | reliable - -mqtt_to_ros2: [] - # - mqtt_topic: example/mqtt_topic - # ros_topic: /example/ros_topic - # msg_type: std_msgs/msg/String - # format: cdr - # qos_depth: 10 - # qos_durability: volatile - # qos_reliability: best_effort +# Northbound: ROS 2 -> MQTT -> Supervision ingest -> gRPC ClientStream. +# `raw` publishes msg.data verbatim, so the ROS String must already hold the +# gateway JSON. Anything else nests it inside a bridge envelope. +# The imoger_dlr ACL grants od_imoger/# only. +ros2_to_mqtt: + - ros_topic: /supervision/telemetry + mqtt_topic: od_imoger/adore/MV-001/telemetry + msg_type: std_msgs/msg/String + format: raw + qos_depth: 1 + qos_reliability: best_effort + qos_durability: volatile + + - ros_topic: /supervision/notifications + mqtt_topic: od_imoger/adore/MV-001/notifications + msg_type: std_msgs/msg/String + format: raw + qos_depth: 20 + qos_reliability: reliable + qos_durability: volatile + +# Loopback for local verification: whatever lands on the northbound topics comes +# back into ROS so `ros2 topic echo` can inspect the exact bytes on the wire. +mqtt_to_ros2: + - mqtt_topic: od_imoger/adore/MV-001/telemetry + ros_topic: /supervision/telemetry_echo + msg_type: std_msgs/msg/String + format: raw + qos_depth: 1 + qos_reliability: reliable + qos_durability: transient_local + + - mqtt_topic: od_imoger/adore/MV-001/notifications + ros_topic: /supervision/notifications_echo + msg_type: std_msgs/msg/String + format: raw + qos_depth: 20 + qos_reliability: reliable + qos_durability: transient_local + + - mqtt_topic: od_imoger/solbox/+/notifications + ros_topic: /imoger/solbox/notifications + msg_type: std_msgs/msg/String + format: raw + qos_depth: 20 + qos_reliability: reliable + + - mqtt_topic: od_imoger/vehicles/dlr1/nmea + ros_topic: /imoger/vehicles/dlr1/nmea + msg_type: std_msgs/msg/String + format: raw + qos_depth: 50 + qos_reliability: best_effort diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config_diamos.yaml b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config_diamos.yaml deleted file mode 100644 index ec6401b4..00000000 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config_diamos.yaml +++ /dev/null @@ -1,70 +0,0 @@ -mqtt: - host: localhost - port: 1883 - # host_env: MQTT_HOST - # port_env: MQTT_PORT - keepalive: 60 - transport: tcp # tcp | websockets - protocol: mqtt # mqtt (v3.1.1) | mqttv5 - env_file: ../.mqtt_secrets.env - - auth: - username_env: MQTT_USERNAME - password_env: MQTT_PASSWORD - - # TLS configuration. - # enabled / ca_certs / certfile / keyfile all support _env variants, - # e.g. enabled_env: MQTT_TLS, ca_certs_env: MQTT_CA_CERT, etc. - # Relative paths resolve against MQTT_BRIDGE_CERT_DIR. - # The broker enforces mTLS, so certfile and keyfile are both required. - tls: - enabled: true - ca_certs: broker.crt - certfile: client.crt - keyfile: client.key - - # Set true only to disable hostname verification (testing only). - insecure: false - - reconnect_delay: 1 - reconnect_max_delay: 120 - -# Northbound: ROS 2 -> MQTT -> Supervision ingest -> gRPC ClientStream. -# `raw` publishes msg.data verbatim, so the ROS String must already hold the -# gateway JSON. Anything else nests it inside a bridge envelope. -# The imoger_dlr ACL grants od_imoger/# only. -ros2_to_mqtt: - - ros_topic: /supervision/telemetry - mqtt_topic: od_imoger/adore/MV-001/telemetry - msg_type: std_msgs/msg/String - format: raw - qos_depth: 1 - qos_reliability: best_effort - qos_durability: volatile - - - ros_topic: /supervision/notifications - mqtt_topic: od_imoger/adore/MV-001/notifications - msg_type: std_msgs/msg/String - format: raw - qos_depth: 20 - qos_reliability: reliable - qos_durability: volatile - -# Loopback for local verification: whatever lands on the northbound topics comes -# back into ROS so `ros2 topic echo` can inspect the exact bytes on the wire. -mqtt_to_ros2: - - mqtt_topic: od_imoger/adore/MV-001/telemetry - ros_topic: /supervision/telemetry_echo - msg_type: std_msgs/msg/String - format: raw - qos_depth: 1 - qos_reliability: reliable - qos_durability: transient_local - - - mqtt_topic: od_imoger/adore/MV-001/notifications - ros_topic: /supervision/notifications_echo - msg_type: std_msgs/msg/String - format: raw - qos_depth: 20 - qos_reliability: reliable - qos_durability: transient_local diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py index dfc44a87..e5fec694 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py @@ -8,7 +8,7 @@ from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy -from .utils import load_msg_type, msg_to_bytes, bytes_to_msg, msg_to_json, json_to_msg, msg_to_cdr_json, cdr_json_to_msg, ensure_self_signed_cert +from .utils import load_msg_type, msg_to_bytes, bytes_to_msg, msg_to_json, json_to_msg, msg_to_cdr_json, cdr_json_to_msg, raw_to_str_msg, str_msg_to_raw, ensure_self_signed_cert _STR_TYPE = 'std_msgs/msg/String' @@ -20,12 +20,29 @@ def _resolve(path, base=_CERT_DIR): return path return os.path.join(base, path) +def _package_root(start_path: str): + """Nearest ancestor of start_path containing package.xml. + + realpath() is used so that a symlink-installed config resolves back into + the source tree rather than the install space. + """ + directory = os.path.dirname(os.path.realpath(start_path)) + while True: + if os.path.isfile(os.path.join(directory, 'package.xml')): + return directory + parent = os.path.dirname(directory) + if parent == directory: + return None + directory = parent + _PROTOCOL_MAP = { 'mqtt': mqtt.MQTTv311, 'mqttv5': mqtt.MQTTv5, } def _serializer(ros_type: str, fmt: str): + if fmt == 'raw': + return str_msg_to_raw if fmt == 'json': return lambda msg, rt=ros_type: msg_to_json(msg, rt) if fmt == 'cdr_json': @@ -33,6 +50,8 @@ def _serializer(ros_type: str, fmt: str): return msg_to_bytes def _deserializer(msg_type, fmt: str): + if fmt == 'raw': + return raw_to_str_msg if fmt == 'json': return lambda data, mt=msg_type: json_to_msg(data, mt) if fmt == 'cdr_json': @@ -40,7 +59,7 @@ def _deserializer(msg_type, fmt: str): return lambda data, mt=msg_type: bytes_to_msg(data, mt) def _wire_type(ros_type: str, fmt: str) -> str: - return _STR_TYPE if fmt in ('json', 'cdr_json') else ros_type + return _STR_TYPE if fmt in ('json', 'cdr_json', 'raw') else ros_type _DURABILITY = { 'volatile': DurabilityPolicy.VOLATILE, @@ -83,10 +102,14 @@ def __init__(self): self._m2r_queue = queue.Queue() self._shutdown_event = threading.Event() self._mqtt_topic_map = {} + self._mqtt_wildcards = [] + self._topic_cache = {} + self._pending_subs = {} env_file = self.config.get('mqtt', {}).get('env_file') if env_file and not os.path.isabs(env_file): - env_file = os.path.join(os.path.dirname(os.path.abspath(config_path)), env_file) + base = _package_root(config_path) or os.path.dirname(os.path.abspath(config_path)) + env_file = os.path.normpath(os.path.join(base, env_file)) self._load_env_file(env_file) self._setup_mqtt() self._setup_ros2_to_mqtt() @@ -136,6 +159,7 @@ def _setup_mqtt(self): ) self.mqtt_client.on_connect = self._on_mqtt_connect self.mqtt_client.on_message = self._on_mqtt_message + self.mqtt_client.on_subscribe = self._on_mqtt_subscribe self._configure_auth(cfg) self._configure_tls(cfg) @@ -213,23 +237,51 @@ def _on_mqtt_connect(self, client, userdata, flags, reason_code, properties): if reason_code == 0: self.get_logger().info('Connected to MQTT broker') for mqtt_topic in self._mqtt_topic_map: - client.subscribe(mqtt_topic) - self.get_logger().info(f'Subscribed to MQTT topic: {mqtt_topic}') + self._subscribe(mqtt_topic) else: self.get_logger().error(f'MQTT connection failed with code: {reason_code}') + def _subscribe(self, mqtt_topic: str): + result, mid = self.mqtt_client.subscribe(mqtt_topic) + self._pending_subs[mid] = mqtt_topic + self.get_logger().info(f'SUBSCRIBE sent for: {mqtt_topic}') + + def _on_mqtt_subscribe(self, client, userdata, mid, reason_codes, properties): + topic = self._pending_subs.pop(mid, f'') + for reason in reason_codes: + if reason.is_failure: + self.get_logger().error(f'SUBSCRIBE denied for {topic}: {reason}') + else: + self.get_logger().info(f'SUBSCRIBE granted for {topic} (QoS {reason.value})') + def _on_mqtt_message(self, client, userdata, message): if self._shutdown_event.is_set(): return - entry = self._mqtt_topic_map.get(message.topic) + entry = self._resolve_topic(message.topic) if entry is None: return + self.get_logger().debug(f'M2R: {message.topic} ({len(message.payload)} bytes)') pub, msg_type, ros_topic, deserialize = entry try: self._m2r_queue.put((pub, deserialize(message.payload))) except Exception as e: self.get_logger().error(f'Deser failed on {ros_topic}: {e}') + def _resolve_topic(self, topic: str): + entry = self._mqtt_topic_map.get(topic) + if entry is not None: + return entry + if topic in self._topic_cache: + return self._topic_cache[topic] + for topic_filter in self._mqtt_wildcards: + if mqtt.topic_matches_sub(topic_filter, topic): + entry = self._mqtt_topic_map[topic_filter] + break + if entry is None: + self.get_logger().warning(f'No mapping matched inbound MQTT topic: {topic}') + self._topic_cache[topic] = entry + return entry + def _setup_ros2_to_mqtt(self): for mapping in self.config.get('ros2_to_mqtt', []): ros_topic = mapping['ros_topic'] @@ -258,10 +310,12 @@ def _setup_mqtt_to_ros2(self): deserialize = _deserializer(m_type, fmt) self.ros_pubs[mqtt_topic] = pub self._mqtt_topic_map[mqtt_topic] = (pub, m_type, ros_topic, deserialize) + if '+' in mqtt_topic or '#' in mqtt_topic: + self._mqtt_wildcards.append(mqtt_topic) if self.mqtt_client.is_connected(): for mqtt_topic in self._mqtt_topic_map: - self.mqtt_client.subscribe(mqtt_topic) + self._subscribe(mqtt_topic) def _drain_m2r_queue(self): while not self._m2r_queue.empty(): diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/utils.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/utils.py index d40bd4d2..78a5ba66 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/utils.py +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/utils.py @@ -118,3 +118,15 @@ def cdr_json_to_msg(data: bytes, msg_type): msg = msg_type() set_message_fields(msg, obj) return msg + + +def raw_to_str_msg(data: bytes): + """Wrap an opaque MQTT payload verbatim in std_msgs/msg/String.""" + from std_msgs.msg import String + msg = String() + msg.data = data.decode('utf-8', errors='replace') + return msg + + +def str_msg_to_raw(msg) -> bytes: + return msg.data.encode('utf-8') From 3e839988a926eb361beb59fc5f836480446f11a0 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Mon, 10 Aug 2026 14:02:02 +0200 Subject: [PATCH 14/18] refactored mqtt bridge node, added dockerfile and makefile for testing. Signed-off-by: Andrew Koerner --- .../mqtt_message_bridge/.dockerignore | 8 + .../mqtt_message_bridge/.gitignore | 4 + .../mqtt_message_bridge/Dockerfile | 30 +++ .../mqtt_message_bridge/Makefile | 32 +++ .../mqtt_message_bridge/README.md | 56 ++++- .../mqtt_message_bridge/bridge_config.yaml | 1 - .../config/bridge_config.yaml | 10 +- .../mqtt_message_bridge/docker-entrypoint.sh | 24 ++ .../echo_test_ros_message.sh | 1 + .../mqtt_message_bridge/bridge_node.py | 81 +++++-- .../mqtt_message_bridge/diagnostics.py | 60 +++++ .../mqtt_message_bridge/mqtt_publish.py | 22 -- .../mqtt_message_bridge/mqtt_subscribe.py | 24 -- .../publish_test_ros_message.sh | 2 + .../scripts/bridge_mqtt_args.py | 98 +++------ .../scripts/dimos_message.py | 148 +++++++++++++ .../scripts/mqtt_broker.py | 193 +++++++++++++++++ .../scripts/mqtt_check_broker.sh | 4 +- .../scripts/mqtt_common.sh | 7 +- .../mqtt_message_bridge/scripts/mqtt_log.sh | 0 .../scripts/mqtt_log_writer.py | 0 .../mqtt_message_bridge/scripts/mqtt_probe.py | 122 +++++++++++ .../scripts/mqtt_publish.py | 130 +++++++++++ .../scripts/mqtt_subscribe.py | 159 ++++++++++++++ .../scripts/mqtt_test_bridge.sh | 83 ++++--- .../scripts/mqtt_test_remote.sh | 205 ++++++++++++++++++ .../scripts/publish_vehicle_state.py | 0 .../mqtt_message_bridge/scripts/run_tests.sh | 197 +++++++++++++++++ .../{ => scripts}/start_local_mqtt_broker.sh | 0 .../{ => scripts}/stop_local_mqtt_broker.sh | 0 .../scripts/test_common.sh | 109 ++++++++++ .../mqtt_message_bridge/test/__init__.py | 0 .../test/bridge_config.test.yaml | 48 ++++ .../test/test_broker_settings.py | 159 ++++++++++++++ .../test/test_dimos_message.py | 104 +++++++++ .../mqtt_message_bridge/test/test_utils.py | 75 +++++++ setup.sh | 2 +- tools/start_mqtt_bridge.sh | 39 ++++ tools/start_mqtt_broker.sh | 33 ++- 39 files changed, 2081 insertions(+), 189 deletions(-) create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.dockerignore create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/Dockerfile create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/Makefile delete mode 120000 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/bridge_config.yaml create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/docker-entrypoint.sh create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/echo_test_ros_message.sh create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/diagnostics.py delete mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_publish.py delete mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_subscribe.py create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/publish_test_ros_message.sh mode change 100644 => 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/bridge_mqtt_args.py create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/dimos_message.py create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_broker.py mode change 100755 => 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh mode change 100644 => 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log.sh mode change 100644 => 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log_writer.py create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_probe.py create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_publish.py create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_subscribe.py create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_remote.sh mode change 100644 => 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/publish_vehicle_state.py create mode 100755 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/run_tests.sh rename ros2_workspace/src/adore_interfaces/mqtt_message_bridge/{ => scripts}/start_local_mqtt_broker.sh (100%) rename ros2_workspace/src/adore_interfaces/mqtt_message_bridge/{ => scripts}/stop_local_mqtt_broker.sh (100%) create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/test_common.sh create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/__init__.py create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/bridge_config.test.yaml create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_broker_settings.py create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_dimos_message.py create mode 100644 ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_utils.py diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.dockerignore b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.dockerignore new file mode 100644 index 00000000..178971a1 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.dockerignore @@ -0,0 +1,8 @@ +build/ +install/ +log/ +**/__pycache__/ +**/*.pyc +.git/ +.gitignore +*.jsonl diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore index c37ec263..29695ac1 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/.gitignore @@ -4,3 +4,7 @@ mqtt_message_bridge/__pycache__ *.key certs/ !certs/.gitkeep +build/ +install/ +log/ +*.jsonl diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/Dockerfile b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/Dockerfile new file mode 100644 index 00000000..2834a1a5 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/Dockerfile @@ -0,0 +1,30 @@ +ARG ROS_DISTRO=jazzy +FROM ros:${ROS_DISTRO}-ros-base +ARG ROS_DISTRO=jazzy + +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3-pip \ + python3-yaml \ + python3-numpy \ + python3-cryptography \ + mosquitto \ + mosquitto-clients \ + netcat-openbsd \ + && rm -rf /var/lib/apt/lists/* + +# Noble ships python3-paho-mqtt 1.6, which predates CallbackAPIVersion.VERSION2. +RUN pip3 install --no-cache-dir --break-system-packages 'paho-mqtt>=2.0.0' + +WORKDIR /ws +COPY . /ws/src/mqtt_message_bridge/ + +ENV MQTT_BRIDGE_CERT_DIR=/ws/src/mqtt_message_bridge/certs + +RUN . /opt/ros/${ROS_DISTRO}/setup.sh && \ + cd /ws && \ + colcon build --packages-select mqtt_message_bridge + +COPY docker-entrypoint.sh /docker-entrypoint.sh +RUN chmod +x /docker-entrypoint.sh +ENTRYPOINT ["/docker-entrypoint.sh"] +CMD ["bridge"] diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/Makefile b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/Makefile new file mode 100644 index 00000000..6a6fd9a2 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/Makefile @@ -0,0 +1,32 @@ +IMAGE ?= mqtt_message_bridge +TAG ?= latest +ROS_DISTRO ?= jazzy + +.PHONY: build test test-offline test-host clean run shell + +build: + docker build --build-arg ROS_DISTRO=$(ROS_DISTRO) -t $(IMAGE):$(TAG) . + +# Local suite against a throwaway broker, then the real broker: reachability, +# TLS, auth, live NMEA data and a mock solbox notification through to ROS. +test: build + docker run --rm $(IMAGE):$(TAG) test + +# Local suite only, for machines without egress to the broker. +test-offline: build + docker run --rm $(IMAGE):$(TAG) test --no-remote + +run: build + docker run --rm -it --network host $(IMAGE):$(TAG) bridge + +shell: build + docker run --rm -it $(IMAGE):$(TAG) shell + +test-host: + ./scripts/run_tests.sh + +clean: + -docker rmi -f $(IMAGE):$(TAG) + rm -rf build install log + find . -name '__pycache__' -type d -prune -exec rm -rf {} + + find . -name '*.pyc' -delete diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/README.md b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/README.md index 5e397395..f43d98b3 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/README.md +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/README.md @@ -26,6 +26,26 @@ mqtt_to_ros2: Each mapping supports optional QoS overrides: `qos_depth`, `qos_durability` (`volatile`|`transient_local`), `qos_reliability` (`best_effort`|`reliable`). +## DiMOS 5.2 interface + +`config/bridge_config.yaml` targets the UseCase 5.2 broker +(`mqtts://broker-imoger.dev.dimos-ops.com:8887`, MQTT v3.1.1, QoS 1) using the +certificate file names the specification hands out: `imoger-rootCA.crt`, +`dlr-client.crt`, `dlr-client.key`. Credentials come from `.mqtt_secrets.env`. + +Subscribed topics: + +| MQTT | ROS 2 | +| --- | --- | +| `od_imoger/solbox/+/notifications` | `/imoger/solbox/notifications` | +| `od_imoger/vehicles/dlr1/nmea` | `/imoger/vehicles/dlr1/nmea` | + +Payloads are UTF-8 JSON and are bridged verbatim (`format: raw`) into +`std_msgs/msg/String`. `scripts/dimos_message.py` builds a notification message +and checks a received one for the fields the bridge relies on. It validates +structure and types only; field values are left to the publisher, since the live +traffic uses cause codes, station types and intervals outside any fixed list. + ## Launch ```bash @@ -35,9 +55,37 @@ ros2 launch mqtt_message_bridge bridge.launch.py mqtt_broker:=localhost mqtt_por ## Test ```bash -python3 mqtt_publish.py # publishes to mqtt/chatter -python3 mqtt_subscribe.py # subscribes to mqtt/chatter -ros2 topic echo /mqtt_chatter +make build # docker image +make test # local suite, then the remote broker suite +make test-offline # local suite only +make clean +``` + +The remote suite works down the stack against the broker in +`config/bridge_config.yaml`, so a failure names the layer that broke: + +1. host reachable (TCP) +2. TLS handshake, reporting the negotiated version and the server certificate +3. authentication, reporting the CONNACK reason code +4. live data on `od_imoger/vehicles/dlr1/nmea`, plus a synthetic notification + published to `od_imoger/solbox/solbox_test/notifications` +5. the same data arriving on `/imoger/vehicles/dlr1/nmea` and + `/imoger/solbox/notifications` through the bridge + +One received payload is printed at each stage. Stages below a failure are not +attempted. Missing credentials or certificates skip rather than fail. `NMEA_WAIT` +allows longer for live data; `NMEA_REQUIRE=0` treats a silent vehicle as a skip. + +Individually, against whatever broker `config/bridge_config.yaml` names: + +```bash +./scripts/mqtt_check_broker.sh +./scripts/mqtt_test_pubsub.sh +python3 scripts/mqtt_publish.py mqtt/chatter +python3 scripts/mqtt_subscribe.py mqtt/chatter ``` -`mqtt_publish.py` and `mqtt_subscribe.py` respect `MQTT_BROKER_HOST` and `MQTT_BROKER_PORT` environment variables. +The scripts read the same config as the node, so TLS and credentials are picked +up automatically. `MQTT_HOST`, `MQTT_PORT`, `MQTT_TLS`, `MQTT_USERNAME` and +`MQTT_PASSWORD` override the file. `--format raw` skips the CDR layer and works +without a sourced ROS workspace. diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/bridge_config.yaml b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/bridge_config.yaml deleted file mode 120000 index 8095fd10..00000000 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/bridge_config.yaml +++ /dev/null @@ -1 +0,0 @@ -config/bridge_config.yaml \ No newline at end of file diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml index 4c63c424..176fcc95 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/config/bridge_config.yaml @@ -8,6 +8,9 @@ mqtt: protocol: mqtt # mqtt (v3.1.1) | mqttv5 env_file: .mqtt_secrets.env + # DiMOS 5.2 specifies QoS 1 (at least once). Per-mapping mqtt_qos overrides it. + qos: 1 + auth: username_env: MQTT_USERNAME password_env: MQTT_PASSWORD @@ -17,11 +20,12 @@ mqtt: # e.g. enabled_env: MQTT_TLS, ca_certs_env: MQTT_CA_CERT, etc. # Relative paths resolve against MQTT_BRIDGE_CERT_DIR. # The broker enforces mTLS, so certfile and keyfile are both required. + # File names are the ones the DiMOS 5.2 interface specification hands out. tls: enabled: true - ca_certs: broker.crt - certfile: client.crt - keyfile: client.key + ca_certs: imoger-rootCA.crt + certfile: dlr-client.crt + keyfile: dlr-client.key # Set true only to disable hostname verification (testing only). insecure: false diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/docker-entrypoint.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/docker-entrypoint.sh new file mode 100755 index 00000000..49f27df0 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/docker-entrypoint.sh @@ -0,0 +1,24 @@ +#!/usr/bin/env bash +set -e + +source "/opt/ros/${ROS_DISTRO}/setup.bash" +source /ws/install/setup.bash + +SRC=/ws/src/mqtt_message_bridge + +case "${1:-bridge}" in + bridge) + shift || true + exec ros2 launch mqtt_message_bridge bridge.launch.py "$@" + ;; + test) + shift || true + exec "$SRC/scripts/run_tests.sh" "$@" + ;; + shell) + exec bash + ;; + *) + exec "$@" + ;; +esac diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/echo_test_ros_message.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/echo_test_ros_message.sh new file mode 100644 index 00000000..860e42f7 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/echo_test_ros_message.sh @@ -0,0 +1 @@ +ros2 topic echo --full-length /imoger/solbox/notifications diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py index e5fec694..5c13cfd9 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/bridge_node.py @@ -8,6 +8,7 @@ from rclpy.node import Node from rclpy.executors import MultiThreadedExecutor from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, HistoryPolicy +from .diagnostics import failure_hints, is_auth_failure, reason_text from .utils import load_msg_type, msg_to_bytes, bytes_to_msg, msg_to_json, json_to_msg, msg_to_cdr_json, cdr_json_to_msg, raw_to_str_msg, str_msg_to_raw, ensure_self_signed_cert _STR_TYPE = 'std_msgs/msg/String' @@ -86,15 +87,13 @@ def __init__(self): config_path = self.get_parameter('config_path').get_parameter_value().string_value if not config_path or not os.path.exists(config_path): - self.get_logger().error(f"Config file not found: {config_path}") - return + raise SystemExit(f'Config file not found: {config_path or ""}') try: with open(config_path, 'r') as f: - self.config = yaml.safe_load(f) + self.config = yaml.safe_load(f) or {} except Exception as e: - self.get_logger().error(f"Failed to load config: {e}") - return + raise SystemExit(f'Failed to load config {config_path}: {e}') self.mqtt_client = None self.ros_subs = [] @@ -105,6 +104,12 @@ def __init__(self): self._mqtt_wildcards = [] self._topic_cache = {} self._pending_subs = {} + self._mqtt_qos = {} + self._default_qos = 0 + self._host = '' + self._port = 0 + self._tls_enabled = False + self._ever_connected = False env_file = self.config.get('mqtt', {}).get('env_file') if env_file and not os.path.isabs(env_file): @@ -117,6 +122,10 @@ def __init__(self): self.mqtt_client.loop_start() self.create_timer(0.01, self._drain_m2r_queue) + @property + def broker(self) -> str: + return f'{self._host}:{self._port}' + def _load_env_file(self, env_file: str | None): if not env_file: return @@ -150,6 +159,7 @@ def _setup_mqtt(self): port = int(self._env_or(cfg, 'port', 1883, env_var='MQTT_PORT')) keepalive = int(self._env_or(cfg, 'keepalive', 60)) transport = self._env_or(cfg, 'transport', 'tcp') + self._default_qos = int(self._env_or(cfg, 'qos', 0, env_var='MQTT_QOS')) protocol = _PROTOCOL_MAP.get(self._env_or(cfg, 'protocol', 'mqtt'), mqtt.MQTTv311) self.mqtt_client = mqtt.Client( @@ -158,6 +168,7 @@ def _setup_mqtt(self): transport=transport, ) self.mqtt_client.on_connect = self._on_mqtt_connect + self.mqtt_client.on_disconnect = self._on_mqtt_disconnect self.mqtt_client.on_message = self._on_mqtt_message self.mqtt_client.on_subscribe = self._on_mqtt_subscribe @@ -165,8 +176,19 @@ def _setup_mqtt(self): self._configure_tls(cfg) self._configure_reconnect(cfg) - self.mqtt_client.connect(host, port, keepalive=keepalive) - self.get_logger().info(f'Connecting to MQTT broker: {host}:{port}') + self._host, self._port = host, port + self._tls_enabled = bool(getattr(self.mqtt_client, '_ssl_context', None)) + self.get_logger().info( + f'Connecting to MQTT broker {self.broker} ' + f'(tls={"on" if self._tls_enabled else "off"}, transport={transport}, keepalive={keepalive}s)' + ) + try: + self.mqtt_client.connect(host, port, keepalive=keepalive) + except Exception as e: + self.get_logger().error(f'Cannot connect to MQTT broker {self.broker}: {e}') + for hint in failure_hints(host, port, self._tls_enabled): + self.get_logger().error(hint) + raise def _configure_auth(self, cfg: dict): auth = cfg.get('auth') @@ -235,22 +257,39 @@ def _configure_reconnect(self, cfg: dict): def _on_mqtt_connect(self, client, userdata, flags, reason_code, properties): if reason_code == 0: - self.get_logger().info('Connected to MQTT broker') + self._ever_connected = True + self.get_logger().info(f'Connected to MQTT broker {self.broker}') for mqtt_topic in self._mqtt_topic_map: self._subscribe(mqtt_topic) else: - self.get_logger().error(f'MQTT connection failed with code: {reason_code}') + self.get_logger().error( + f'Broker {self.broker} refused the connection: {reason_text(reason_code)}') + if is_auth_failure(reason_code): + self.get_logger().error( + 'Authentication rejected: check MQTT_USERNAME/MQTT_PASSWORD, the client ' + 'certificate and the broker ACL for this identity') + + def _on_mqtt_disconnect(self, client, userdata, flags, reason_code, properties): + if reason_code == 0: + self.get_logger().info(f'Disconnected from MQTT broker {self.broker}') + return + self.get_logger().warning( + f'Unexpected disconnect from {self.broker}: {reason_text(reason_code)}') + if not self._ever_connected: + for hint in failure_hints(self._host, self._port, self._tls_enabled): + self.get_logger().error(hint) def _subscribe(self, mqtt_topic: str): - result, mid = self.mqtt_client.subscribe(mqtt_topic) + qos = self._mqtt_qos.get(mqtt_topic, self._default_qos) + result, mid = self.mqtt_client.subscribe(mqtt_topic, qos=qos) self._pending_subs[mid] = mqtt_topic - self.get_logger().info(f'SUBSCRIBE sent for: {mqtt_topic}') + self.get_logger().info(f'SUBSCRIBE sent for: {mqtt_topic} (qos {qos})') def _on_mqtt_subscribe(self, client, userdata, mid, reason_codes, properties): topic = self._pending_subs.pop(mid, f'') for reason in reason_codes: if reason.is_failure: - self.get_logger().error(f'SUBSCRIBE denied for {topic}: {reason}') + self.get_logger().error(f'SUBSCRIBE denied for {topic}: {reason_text(reason)}') else: self.get_logger().info(f'SUBSCRIBE granted for {topic} (QoS {reason.value})') @@ -290,9 +329,10 @@ def _setup_ros2_to_mqtt(self): msg_type = load_msg_type(ros_type) fmt = mapping.get('format', 'cdr') qos = _qos_from_mapping(mapping) + mqtt_qos = int(mapping.get('mqtt_qos', self._default_qos)) serialize = _serializer(ros_type, fmt) - cb = lambda msg, t=mqtt_topic, s=serialize: ( - self.mqtt_client.publish(t, s(msg)), + cb = lambda msg, t=mqtt_topic, s=serialize, q=mqtt_qos: ( + self.mqtt_client.publish(t, s(msg), qos=q), self.get_logger().debug(f'R2M: {t}') ) self.ros_subs.append(self.create_subscription(msg_type, ros_topic, cb, qos)) @@ -309,6 +349,7 @@ def _setup_mqtt_to_ros2(self): pub = self.create_publisher(pub_type, ros_topic, qos) deserialize = _deserializer(m_type, fmt) self.ros_pubs[mqtt_topic] = pub + self._mqtt_qos[mqtt_topic] = int(mapping.get('mqtt_qos', self._default_qos)) self._mqtt_topic_map[mqtt_topic] = (pub, m_type, ros_topic, deserialize) if '+' in mqtt_topic or '#' in mqtt_topic: self._mqtt_wildcards.append(mqtt_topic) @@ -324,13 +365,19 @@ def _drain_m2r_queue(self): def shutdown(self): self._shutdown_event.set() - self.mqtt_client.loop_stop() - self.mqtt_client.disconnect() + if self.mqtt_client is not None: + self.mqtt_client.loop_stop() + self.mqtt_client.disconnect() def main(args=None): rclpy.init(args=args) - node = ROS2MQTTBridge() + try: + node = ROS2MQTTBridge() + except BaseException as e: + rclpy.shutdown() + raise SystemExit(f'{type(e).__name__}: {e}' if not isinstance(e, SystemExit) else e) + executor = MultiThreadedExecutor() executor.add_node(node) try: diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/diagnostics.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/diagnostics.py new file mode 100644 index 00000000..89f8fa4a --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_message_bridge/diagnostics.py @@ -0,0 +1,60 @@ +"""Connection failure diagnostics. + +Imports nothing from rclpy or paho so it can be used by the node and by the +standalone scripts alike. +""" +import socket +import ssl + +AUTH_FAILURE_CODES = (4, 5, 134, 135) + + +def reason_text(reason_code) -> str: + """paho 2.x hands back a ReasonCode object, which has no __int__.""" + name = getattr(reason_code, 'getName', None) + value = getattr(reason_code, 'value', reason_code) + return f'{name() if name else reason_code} (code {value})' + + +def is_auth_failure(reason_code) -> bool: + return getattr(reason_code, 'value', reason_code) in AUTH_FAILURE_CODES + + +def port_speaks_tls(host: str, port: int, timeout: float = 3.0) -> bool | None: + """True if the port completes a TLS handshake, False if it is plaintext, + None if the TCP connection could not be established at all.""" + context = ssl.create_default_context() + context.check_hostname = False + context.verify_mode = ssl.CERT_NONE + try: + raw = socket.create_connection((host, port), timeout=timeout) + except OSError: + return None + with raw: + try: + with context.wrap_socket(raw): + return True + except OSError: + return False + + +def failure_hints(host: str, port: int, tls_enabled: bool, missing_files=()) -> list: + """Actionable lines explaining why a connection to host:port did not work.""" + address = f'{host}:{port}' + hints = [] + if missing_files: + hints.append(f'TLS material missing on disk: {", ".join(missing_files)}') + hints.append('set MQTT_BRIDGE_CERT_DIR or correct the paths under mqtt.tls') + + speaks_tls = port_speaks_tls(host, port) + if speaks_tls is None: + hints.append(f'{address} refused a plain TCP probe; check the host, port and firewall') + elif speaks_tls and not tls_enabled: + hints.append(f'{address} speaks TLS but this client connected in plaintext, so the broker ' + f'response decodes as garbage; enable mqtt.tls or set MQTT_TLS=1') + elif not speaks_tls and tls_enabled: + hints.append(f'{address} is a plaintext listener but this client attempted TLS; set MQTT_TLS=0') + else: + hints.append(f'{address} accepted a transport-level connection, so the failure is above ' + f'the transport: credentials, client certificate or broker ACL') + return hints diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_publish.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_publish.py deleted file mode 100644 index 1e9860d3..00000000 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_publish.py +++ /dev/null @@ -1,22 +0,0 @@ -import os -import time -import rclpy -import paho.mqtt.client as mqtt -from rclpy.serialization import serialize_message -from std_msgs.msg import String - -rclpy.init() - -host = os.environ.get("MQTT_BROKER_HOST", "127.0.0.1") -port = int(os.environ.get("MQTT_BROKER_PORT", 1883)) - -client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2) -client.connect(host, port) -client.loop_start() - -msg = String() -while True: - msg.data = "Hello, MQTT!" - client.publish("mqtt/chatter", serialize_message(msg)) - print("Published: Hello, MQTT!") - time.sleep(1) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_subscribe.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_subscribe.py deleted file mode 100644 index dab29900..00000000 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/mqtt_subscribe.py +++ /dev/null @@ -1,24 +0,0 @@ -import os -import rclpy -import paho.mqtt.client as mqtt -from rclpy.serialization import deserialize_message -from std_msgs.msg import String - -rclpy.init() - -host = os.environ.get("MQTT_BROKER_HOST", "127.0.0.1") -port = int(os.environ.get("MQTT_BROKER_PORT", 1883)) - -def on_connect(client, userdata, flags, reason_code, properties): - if reason_code == 0: - client.subscribe("mqtt/chatter") - -def on_message(client, userdata, message): - msg = deserialize_message(message.payload, String) - print(f"[{message.topic}] {msg.data}") - -client = mqtt.Client(mqtt.CallbackAPIVersion.VERSION2) -client.on_connect = on_connect -client.on_message = on_message -client.connect(host, port) -client.loop_forever() diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/publish_test_ros_message.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/publish_test_ros_message.sh new file mode 100644 index 00000000..c1dad128 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/publish_test_ros_message.sh @@ -0,0 +1,2 @@ +ros2 topic pub -1 /imoger/solbox/notifications std_msgs/msg/String \ + "{data: '{\"header\": {\"protocol_ver\": \"0.1.0\", \"msg_type\": \"warning\", \"origin_station_id\": \"solbox_test\"}, \"payload\": {\"management\": {\"action_id\": 4242, \"detection_time\": \"2026-08-10 10:10:26\", \"reference_time\": \"2026-08-10 10:10:26\", \"termination\": 0, \"event_position\": {\"lat\": 47.0, \"lon\": 12.0, \"alt\": 550}, \"awareness_distance\": 500.0, \"transmission_interval\": 4, \"station_type\": 15}, \"situation\": {\"event_type\": {\"cause_code\": \"pos\"}, \"road_type\": 10}}, \"alacarte\": {\"warning\": {\"continuity\": {\"r_hpl\": 100.0}}}}'}" diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/bridge_mqtt_args.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/bridge_mqtt_args.py old mode 100644 new mode 100755 index a3bcc835..2d1d407c --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/bridge_mqtt_args.py +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/bridge_mqtt_args.py @@ -1,83 +1,45 @@ #!/usr/bin/env python3 +"""Emit NUL-separated mosquitto_pub/mosquitto_sub arguments for a bridge config. + +Consumed by mqtt_common.sh so the shell tooling connects exactly like the node. +""" import os import sys -import yaml - - -def env_or(cfg, key, default=None, env_var=None): - name = cfg.get(f'{key}_env') or env_var - if name: - val = os.environ.get(name) - if val is not None: - return val - return cfg.get(key, default) - - -def load_env_file(path): - if not path or not os.path.exists(path): - return - with open(path) as f: - for line in f: - line = line.strip() - if not line or line.startswith('#') or '=' not in line: - continue - key, _, value = line.partition('=') - os.environ.setdefault(key.strip(), value.strip().strip('"').strip("'")) - -def main(): - config_path = sys.argv[1] - with open(config_path) as f: - cfg = yaml.safe_load(f) or {} - mqtt = cfg.get('mqtt', {}) - - env_file = mqtt.get('env_file') - if env_file and not os.path.isabs(env_file): - env_file = os.path.join(os.path.dirname(os.path.abspath(config_path)), env_file) - load_env_file(env_file) - - cert_dir = os.environ.get('MQTT_BRIDGE_CERT_DIR', '') +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) - def resolve(path): - if not path or os.path.isabs(path): - return path - return os.path.join(cert_dir, path) +from mqtt_broker import load_settings - args = ['-h', str(env_or(mqtt, 'host', 'localhost', 'MQTT_HOST')), - '-p', str(env_or(mqtt, 'port', 1883, 'MQTT_PORT'))] - auth = mqtt.get('auth') or {} - user_env = auth.get('username_env', 'MQTT_USERNAME') - pass_env = auth.get('password_env', 'MQTT_PASSWORD') - username = os.environ.get(user_env) if user_env else None - password = os.environ.get(pass_env) if pass_env else None - if username: - args += ['-u', username] - if password: - args += ['-P', password] - - tls = mqtt.get('tls') or {} - enabled = env_or(tls, 'enabled', False, 'MQTT_TLS') - if isinstance(enabled, str): - enabled = enabled.lower() in ('1', 'true', 'yes') - if enabled: - ca_certs = resolve(env_or(tls, 'ca_certs', env_var='MQTT_CA_CERT')) - certfile = resolve(env_or(tls, 'certfile', env_var='MQTT_CLIENT_CERT')) - keyfile = resolve(env_or(tls, 'keyfile', env_var='MQTT_CLIENT_KEY')) - if ca_certs: - args += ['--cafile', ca_certs] +def broker_args(settings) -> list: + args = ['-h', settings.host, '-p', str(settings.port), '-q', str(settings.qos)] + if settings.username: + args += ['-u', settings.username] + if settings.password: + args += ['-P', settings.password] + if settings.tls: + if settings.ca_certs: + args += ['--cafile', settings.ca_certs] else: args += ['--capath', '/etc/ssl/certs'] - if certfile: - args += ['--cert', certfile] - if keyfile: - args += ['--key', keyfile] - if tls.get('insecure'): + if settings.certfile: + args += ['--cert', settings.certfile] + if settings.keyfile: + args += ['--key', settings.keyfile] + if settings.insecure: args += ['--insecure'] + return args + - for arg in args: +def main(): + settings = load_settings(sys.argv[1] if len(sys.argv) > 1 else None) + if settings.missing: + print(f'ERROR: TLS material missing: {", ".join(settings.missing)}', file=sys.stderr) + return 1 + for arg in broker_args(settings): sys.stdout.write(arg + '\0') + return 0 if __name__ == '__main__': - main() + sys.exit(main()) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/dimos_message.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/dimos_message.py new file mode 100755 index 00000000..7f2d2033 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/dimos_message.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +"""Build and structurally validate solbox notification messages. + + python3 scripts/dimos_message.py --generate [--action-id N] + python3 scripts/dimos_message.py --validate FILE + +Validation checks only that the fields the bridge and its consumers rely on are +present and hold the expected JSON types. Values are not constrained: the +publisher owns them, and the observed traffic uses cause codes, station types +and intervals beyond any fixed list. +""" +import argparse +import json +import random +import sys +from datetime import datetime, timezone + +TIME_FORMAT = '%Y-%m-%d %H:%M:%S' + +NUMBER = (int, float) + +REQUIRED_FIELDS = ( + ('header.protocol_ver', str), + ('header.msg_type', str), + ('header.origin_station_id', (str, int)), + ('payload.management.action_id', (str, int)), + ('payload.management.detection_time', str), + ('payload.management.reference_time', str), + ('payload.management.termination', int), + ('payload.management.event_position.lat', NUMBER), + ('payload.management.event_position.lon', NUMBER), + ('payload.management.event_position.alt', NUMBER), + ('payload.management.awareness_distance', NUMBER), + ('payload.management.transmission_interval', NUMBER), + ('payload.management.station_type', int), + ('payload.situation.event_type.cause_code', str), + ('payload.situation.road_type', int), + ('alacarte', dict), +) + + +def build(msg_type: str = 'warning', station_id: str = 'solbox_test', action_id: int | None = None) -> dict: + now = datetime.now(timezone.utc).strftime(TIME_FORMAT) + action_id = random.randrange(10**8, 10**9) if action_id is None else action_id + message = { + 'header': { + 'protocol_ver': '0.1.0', + 'msg_type': msg_type, + 'origin_station_id': station_id, + }, + 'payload': { + 'management': { + 'action_id': action_id, + 'detection_time': now, + 'reference_time': now, + 'termination': 0, + 'event_position': {'lat': 47.0, 'lon': 12.0, 'alt': 550}, + 'awareness_distance': 500.0, + 'transmission_interval': 4, + 'station_type': 15, + }, + 'situation': { + 'event_type': {'cause_code': 'pos'}, + 'road_type': 10, + }, + }, + 'alacarte': { + 'warning': {'continuity': {'r_hpl': 100.0}}, + }, + } + if msg_type == 'alert': + message['alacarte']['alert'] = { + 'accuracy': {'hpe': 10.0}, + 'integrity': {'hpl': 100.0}, + } + return message + + +def _get(obj, path): + for key in path.split('.'): + if not isinstance(obj, dict) or key not in obj: + return None + obj = obj[key] + return obj + + +def _type_names(types) -> str: + if isinstance(types, type): + return types.__name__ + return ' or '.join(t.__name__ for t in types) + + +def validate(message) -> list: + if not isinstance(message, dict): + return ['message is not a JSON object'] + + errors = [] + for path, types in REQUIRED_FIELDS: + value = _get(message, path) + if value is None: + errors.append(f'missing field: {path}') + elif isinstance(value, bool) or not isinstance(value, types): + errors.append(f'{path} has type {type(value).__name__}, expected {_type_names(types)}') + + for path in ('payload.management.detection_time', 'payload.management.reference_time'): + value = _get(message, path) + if isinstance(value, str): + try: + datetime.strptime(value, TIME_FORMAT) + except ValueError: + errors.append(f'{path} is {value!r}, expected format yyyy-MM-dd hh:mm:ss') + + return errors + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument('--generate', action='store_true') + group.add_argument('--validate', metavar='FILE', help='file to read, or - for stdin') + parser.add_argument('--msg-type', default='warning') + parser.add_argument('--station-id', default='solbox_test') + parser.add_argument('--action-id', type=int, default=None) + args = parser.parse_args() + + if args.generate: + print(json.dumps(build(args.msg_type, args.station_id, args.action_id))) + return 0 + + raw = sys.stdin.read() if args.validate == '-' else open(args.validate).read() + try: + message = json.loads(raw) + except ValueError as exc: + print(f'not valid JSON: {exc}') + return 1 + + errors = validate(message) + for error in errors: + print(f'error: {error}') + if not errors: + cause = _get(message, 'payload.situation.event_type.cause_code') + msg_type = _get(message, 'header.msg_type') + print(f'structure valid (msg_type={msg_type!r}, cause_code={cause!r})') + return 1 if errors else 0 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_broker.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_broker.py new file mode 100644 index 00000000..a6e0868b --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_broker.py @@ -0,0 +1,193 @@ +"""Broker connection settings for the standalone scripts. + +Reads the same bridge_config.yaml the node uses so the scripts and the bridge +never disagree about host, port, credentials or TLS material. +Precedence: process environment > env_file named in the config > config values. +""" +import logging +import os +import sys +from dataclasses import dataclass, field + +import paho.mqtt.client as mqtt +import yaml + +PROTOCOLS = { + 'mqtt': mqtt.MQTTv311, + 'mqttv5': mqtt.MQTTv5, +} + + +def package_root(start_path: str) -> str | None: + directory = os.path.dirname(os.path.realpath(start_path)) + while True: + if os.path.isfile(os.path.join(directory, 'package.xml')): + return directory + parent = os.path.dirname(directory) + if parent == directory: + return None + directory = parent + + +PKG_ROOT = package_root(__file__) or os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +DEFAULT_CONFIG = os.path.join(PKG_ROOT, 'config', 'bridge_config.yaml') + +sys.path.insert(0, PKG_ROOT) +from mqtt_message_bridge.diagnostics import ( # noqa: E402 + failure_hints, + is_auth_failure, + port_speaks_tls, + reason_text, +) + + +def load_env_file(path: str | None) -> None: + if not path or not os.path.exists(path): + return + with open(path) as f: + for line in f: + line = line.strip() + if not line or line.startswith('#') or '=' not in line: + continue + key, _, value = line.partition('=') + os.environ.setdefault(key.strip(), value.strip().strip('"').strip("'")) + + +def env_or(cfg: dict, key: str, default=None, env_var: str | None = None): + name = cfg.get(f'{key}_env') or env_var + if name: + value = os.environ.get(name) + if value is not None: + return value + return cfg.get(key, default) + + +def as_bool(value) -> bool: + if isinstance(value, str): + return value.strip().lower() in ('1', 'true', 'yes', 'on') + return bool(value) + + +@dataclass +class BrokerSettings: + host: str = 'localhost' + port: int = 1883 + keepalive: int = 60 + transport: str = 'tcp' + protocol: int = mqtt.MQTTv311 + username: str | None = None + password: str | None = None + tls: bool = False + ca_certs: str | None = None + certfile: str | None = None + keyfile: str | None = None + insecure: bool = False + qos: int = 0 + config_path: str | None = None + missing: list = field(default_factory=list) + + @property + def address(self) -> str: + return f'{self.host}:{self.port}' + + +def _resolve(path: str | None, cert_dir: str) -> str | None: + if not path or os.path.isabs(path): + return path + return os.path.join(cert_dir, path) + + +def load_settings(config_path: str | None = None) -> BrokerSettings: + config_path = config_path or os.environ.get('MQTT_BRIDGE_CONFIG') or DEFAULT_CONFIG + cfg = {} + if os.path.exists(config_path): + with open(config_path) as f: + cfg = yaml.safe_load(f) or {} + mqtt_cfg = cfg.get('mqtt') or {} + + env_file = mqtt_cfg.get('env_file') + if env_file and not os.path.isabs(env_file): + base = package_root(config_path) or os.path.dirname(os.path.abspath(config_path)) + env_file = os.path.normpath(os.path.join(base, env_file)) + load_env_file(env_file) + + cert_dir = os.environ.get('MQTT_BRIDGE_CERT_DIR') or os.path.join(PKG_ROOT, 'certs') + + auth = mqtt_cfg.get('auth') or {} + tls_cfg = mqtt_cfg.get('tls') or {} + + settings = BrokerSettings( + host=str(env_or(mqtt_cfg, 'host', 'localhost', 'MQTT_HOST')), + port=int(env_or(mqtt_cfg, 'port', 1883, 'MQTT_PORT')), + keepalive=int(env_or(mqtt_cfg, 'keepalive', 60, 'MQTT_KEEPALIVE')), + qos=int(env_or(mqtt_cfg, 'qos', 0, 'MQTT_QOS')), + transport=str(env_or(mqtt_cfg, 'transport', 'tcp')), + protocol=PROTOCOLS.get(str(env_or(mqtt_cfg, 'protocol', 'mqtt')), mqtt.MQTTv311), + username=os.environ.get(auth.get('username_env', 'MQTT_USERNAME')), + password=os.environ.get(auth.get('password_env', 'MQTT_PASSWORD')), + tls=as_bool(env_or(tls_cfg, 'enabled', False, 'MQTT_TLS')), + ca_certs=_resolve(env_or(tls_cfg, 'ca_certs', env_var='MQTT_CA_CERT'), cert_dir), + certfile=_resolve(env_or(tls_cfg, 'certfile', env_var='MQTT_CLIENT_CERT'), cert_dir), + keyfile=_resolve(env_or(tls_cfg, 'keyfile', env_var='MQTT_CLIENT_KEY'), cert_dir), + insecure=as_bool(tls_cfg.get('insecure', False)), + config_path=config_path if os.path.exists(config_path) else None, + ) + + if settings.tls: + settings.missing = [ + p for p in (settings.ca_certs, settings.certfile, settings.keyfile) + if p and not os.path.exists(p) + ] + return settings + + +def make_client(settings: BrokerSettings, client_id: str = '', log: logging.Logger | None = None) -> mqtt.Client: + client = mqtt.Client( + mqtt.CallbackAPIVersion.VERSION2, + client_id=client_id, + protocol=settings.protocol, + transport=settings.transport, + ) + if settings.username: + client.username_pw_set(settings.username, settings.password) + if settings.tls: + client.tls_set( + ca_certs=settings.ca_certs, + certfile=settings.certfile, + keyfile=settings.keyfile, + ) + if settings.insecure: + client.tls_insecure_set(True) + if log is not None: + client.enable_logger(log) + return client + + +def describe(settings: BrokerSettings) -> str: + parts = [ + f'broker={settings.address}', + f'tls={"on" if settings.tls else "off"}', + f'transport={settings.transport}', + f'user={settings.username or ""}', + f'config={settings.config_path or ""}', + ] + if settings.tls: + parts.append(f'ca={settings.ca_certs or "system"}') + parts.append(f'cert={settings.certfile or ""}') + return ' '.join(parts) + + +def diagnose(settings: BrokerSettings, log: logging.Logger) -> None: + """Explain a failed connection in terms of what the port actually offers.""" + log.error('connection settings: %s', describe(settings)) + for hint in failure_hints(settings.host, settings.port, settings.tls, settings.missing): + log.error('%s', hint) + + +def setup_logging(name: str) -> logging.Logger: + logging.basicConfig( + level=os.environ.get('LOG_LEVEL', 'INFO').upper(), + format='%(asctime)s %(levelname)s %(message)s', + stream=sys.stdout, + ) + return logging.getLogger(name) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_check_broker.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_check_broker.sh index f3f092d3..90907a78 100755 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_check_broker.sh +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_check_broker.sh @@ -34,7 +34,5 @@ mosquitto_sub "${_broker_args[@]}" \ echo "" echo "=== Active Subscriptions (sampling \$SYS for 3s) ===" -mosquitto_sub "${_broker_args[@]}" \ - -t '$SYS/#' \ - -W 3 -v 2>/dev/null \ +{ mosquitto_sub "${_broker_args[@]}" -t '$SYS/#' -W 3 -v 2>/dev/null || true; } \ | grep -i 'subscri' || echo "(none reported)" diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh old mode 100755 new mode 100644 index 07de09ae..81924d90 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_common.sh @@ -4,13 +4,13 @@ # Precedence: real environment > env_file in the config > config defaults. # # Usage: source mqtt_common.sh [/path/to/bridge_config.yaml] -# Defaults to /bridge_config.yaml. +# Defaults to $MQTT_BRIDGE_CONFIG, else /config/bridge_config.yaml. _COMMON_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" _PKG_ROOT="$(cd "$_COMMON_DIR/.." && pwd)" export MQTT_BRIDGE_CERT_DIR="${MQTT_BRIDGE_CERT_DIR:-$_PKG_ROOT/certs}" -_config="${1:-$_PKG_ROOT/bridge_config.yaml}" +_config="${1:-${MQTT_BRIDGE_CONFIG:-$_PKG_ROOT/config/bridge_config.yaml}}" if [[ ! -f "$_config" ]]; then echo "ERROR: bridge config not found: $_config" >&2 exit 1 @@ -22,11 +22,12 @@ if [[ ${#_broker_args[@]} -eq 0 ]]; then exit 1 fi -MQTT_HOST=""; MQTT_PORT=""; MQTT_USERNAME="" +MQTT_HOST=""; MQTT_PORT=""; MQTT_USERNAME=""; MQTT_PASSWORD="" for ((_i = 0; _i < ${#_broker_args[@]}; _i++)); do case "${_broker_args[_i]}" in -h) MQTT_HOST="${_broker_args[_i + 1]}" ;; -p) MQTT_PORT="${_broker_args[_i + 1]}" ;; -u) MQTT_USERNAME="${_broker_args[_i + 1]}" ;; + -P) MQTT_PASSWORD="${_broker_args[_i + 1]}" ;; esac done diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log.sh old mode 100644 new mode 100755 diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log_writer.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_log_writer.py old mode 100644 new mode 100755 diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_probe.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_probe.py new file mode 100755 index 00000000..61a61f95 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_probe.py @@ -0,0 +1,122 @@ +#!/usr/bin/env python3 +"""Probe a broker one layer at a time so a failure names the layer that broke. + + python3 scripts/mqtt_probe.py --stage tcp|tls|auth [--config PATH] + +Prints a single line describing the outcome. Exit codes: 0 pass, 1 fail, +2 not applicable (for example TLS when the config does not enable it). +""" +import argparse +import os +import socket +import ssl +import sys +import threading + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +from mqtt_broker import load_settings, make_client, reason_text + +TIMEOUT = 10 +NOT_APPLICABLE = 2 + + +def stage_tcp(settings): + try: + with socket.create_connection((settings.host, settings.port), timeout=TIMEOUT): + return 0, f'{settings.address} accepted a TCP connection' + except socket.gaierror as exc: + return 1, f'cannot resolve {settings.host!r}: {exc}' + except OSError as exc: + return 1, f'cannot reach {settings.address}: {exc}' + + +def _cert_summary(cert) -> str: + if not cert: + return 'server certificate not verified (insecure mode)' + subject = dict(x[0] for x in cert.get('subject', ())) + issuer = dict(x[0] for x in cert.get('issuer', ())) + return (f'server CN={subject.get("commonName", "?")} ' + f'issuer={issuer.get("commonName", "?")} expires={cert.get("notAfter", "?")}') + + +def stage_tls(settings): + if not settings.tls: + return NOT_APPLICABLE, 'TLS is not enabled in the config' + if settings.missing: + return 1, f'TLS material missing on disk: {", ".join(settings.missing)}' + + try: + context = ssl.create_default_context(cafile=settings.ca_certs) + except OSError as exc: + return 1, f'cannot load CA file {settings.ca_certs}: {exc}' + if settings.insecure: + context.check_hostname = False + context.verify_mode = ssl.CERT_NONE + if settings.certfile: + try: + context.load_cert_chain(settings.certfile, settings.keyfile) + except (OSError, ssl.SSLError) as exc: + return 1, f'cannot load client certificate {settings.certfile}: {exc}' + + try: + with socket.create_connection((settings.host, settings.port), timeout=TIMEOUT) as raw: + with context.wrap_socket(raw, server_hostname=settings.host) as tls: + return 0, f'{tls.version()} handshake completed, {_cert_summary(tls.getpeercert())}' + except ssl.SSLCertVerificationError as exc: + return 1, (f'server certificate rejected: {exc.verify_message or exc}; ' + f'check mqtt.tls.ca_certs ({settings.ca_certs or "system store"})') + except ssl.SSLError as exc: + return 1, (f'TLS handshake with {settings.address} failed: {exc}; ' + f'the port may not be a TLS listener, or the client certificate was rejected') + except OSError as exc: + return 1, f'TLS handshake with {settings.address} failed at the transport: {exc}' + + +def stage_auth(settings): + result = {'code': None} + done = threading.Event() + + def on_connect(client, userdata, flags, reason_code, properties): + result['code'] = reason_code + done.set() + + client = make_client(settings) + client.on_connect = on_connect + try: + client.connect(settings.host, settings.port, settings.keepalive) + except OSError as exc: + return 1, f'connection to {settings.address} failed before CONNACK: {exc}' + + client.loop_start() + try: + if not done.wait(TIMEOUT): + return 1, f'no CONNACK from {settings.address} within {TIMEOUT}s' + code = result['code'] + if code == 0: + identity = settings.username or 'anonymous' + return 0, f'{settings.address} accepted the session as {identity}' + return 1, (f'{settings.address} refused the session: {reason_text(code)}; ' + f'check the username, password, client certificate and broker ACL') + finally: + client.loop_stop() + client.disconnect() + + +STAGES = {'tcp': stage_tcp, 'tls': stage_tls, 'auth': stage_auth} + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--stage', required=True, choices=sorted(STAGES)) + parser.add_argument('--config', default=None) + args = parser.parse_args() + + settings = load_settings(args.config) + status, message = STAGES[args.stage](settings) + print(message) + return status + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_publish.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_publish.py new file mode 100755 index 00000000..0c05efa5 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_publish.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +"""Publish a ROS message to an MQTT topic on an interval. + + python3 scripts/mqtt_publish.py [topic] [--message TEXT] [--count N] +""" +import argparse +import os +import socket +import ssl +import sys +import time + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +from mqtt_broker import describe, diagnose, load_settings, make_client, reason_text, setup_logging + +log = setup_logging('mqtt_publish') + + +def _encode(text: str, fmt: str) -> bytes: + """The ROS import is lazy so --format raw works without a sourced workspace.""" + if fmt == 'raw': + return text.encode('utf-8') + try: + from rclpy.serialization import serialize_message + from std_msgs.msg import String + except ImportError as exc: + raise SystemExit(f'--format cdr needs a sourced ROS 2 workspace ({exc}); ' + f'use --format raw for opaque payloads') + return serialize_message(String(data=text)) + + +def parse_args(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('topic', nargs='?', default='mqtt/chatter') + parser.add_argument('--config', default=None, help='bridge_config.yaml to read broker settings from') + parser.add_argument('--message', default='Hello, MQTT!') + parser.add_argument('--format', default='cdr', choices=('cdr', 'raw')) + parser.add_argument('--count', type=int, default=0, help='messages to send (0 = run forever)') + parser.add_argument('--interval', type=float, default=1.0) + parser.add_argument('--qos', type=int, default=0, choices=(0, 1, 2)) + return parser.parse_args() + + +def main() -> int: + args = parse_args() + settings = load_settings(args.config) + + payload = _encode(args.message, args.format) + client = make_client(settings, log=log) + + state = {'connected': False, 'attempts': 0} + + def on_connect(client, userdata, flags, reason_code, properties): + if reason_code == 0: + state['connected'] = True + state['attempts'] = 0 + log.info('connected to %s', settings.address) + else: + log.error('broker %s refused the connection: %s', settings.address, reason_text(reason_code)) + client.disconnect() + + def on_disconnect(client, userdata, flags, reason_code, properties): + if reason_code == 0: + return + state['attempts'] += 1 + log.warning('unexpected disconnect from %s: %s', settings.address, reason_text(reason_code)) + if not state['connected'] and state['attempts'] == 1: + diagnose(settings, log) + + client.on_connect = on_connect + client.on_disconnect = on_disconnect + + log.info('connecting: %s', describe(settings)) + if settings.missing: + log.error('TLS material missing on disk: %s', ', '.join(settings.missing)) + return 1 + + try: + client.connect(settings.host, settings.port, settings.keepalive) + except socket.gaierror as exc: + log.error('cannot resolve host %r: %s', settings.host, exc) + diagnose(settings, log) + return 1 + except ssl.SSLError as exc: + log.error('TLS handshake with %s failed: %s', settings.address, exc) + diagnose(settings, log) + return 1 + except OSError as exc: + log.error('cannot reach broker at %s: %s', settings.address, exc) + diagnose(settings, log) + return 1 + + client.loop_start() + + deadline = time.monotonic() + 10 + while not state['connected'] and time.monotonic() < deadline: + time.sleep(0.1) + if not state['connected']: + log.error('no CONNACK from %s within 10s', settings.address) + client.loop_stop() + return 1 + + sent = 0 + status = 0 + try: + while not args.count or sent < args.count: + info = client.publish(args.topic, payload, qos=args.qos) + if args.qos: + info.wait_for_publish(timeout=5) + if info.rc != 0: + log.error('publish to %s failed: %s', args.topic, reason_text(info.rc)) + status = 1 + break + sent += 1 + log.info('published %d bytes to %s (%d total)', len(payload), args.topic, sent) + if args.count and sent >= args.count: + break + time.sleep(args.interval) + except KeyboardInterrupt: + log.info('interrupted after %d message(s)', sent) + finally: + client.loop_stop() + client.disconnect() + + return status + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_subscribe.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_subscribe.py new file mode 100755 index 00000000..4d559ea6 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_subscribe.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 +"""Subscribe to an MQTT topic and print each payload. + +Broker settings come from config/bridge_config.yaml, so TLS and credentials +match the bridge node. Environment variables still win over the file. + + python3 scripts/mqtt_subscribe.py [topic] [--format raw|cdr] [--count N] +""" +import argparse +import os +import socket +import ssl +import sys +import threading + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +from mqtt_broker import ( + describe, + diagnose, + is_auth_failure, + load_settings, + make_client, + reason_text, + setup_logging, +) + +MAX_ATTEMPTS = 3 +CONNECT_TIMEOUT = 15 + +log = setup_logging('mqtt_subscribe') + + +def cdr_decoder(): + """Imported lazily so --format raw works without a sourced ROS workspace.""" + try: + from rclpy.serialization import deserialize_message + from std_msgs.msg import String + except ImportError as exc: + raise SystemExit(f'--format cdr needs a sourced ROS 2 workspace ({exc}); ' + f'use --format raw for opaque payloads') + return lambda payload: deserialize_message(payload, String).data + + +def parse_args(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('topic', nargs='?', default='mqtt/chatter') + parser.add_argument('--config', default=None, help='bridge config to read broker settings from') + parser.add_argument('--format', default='cdr', choices=('cdr', 'raw'), + help='cdr deserializes a std_msgs/String, raw prints the payload as text') + parser.add_argument('--count', type=int, default=0, help='exit after N messages (0 = run forever)') + return parser.parse_args() + + +def main() -> int: + args = parse_args() + settings = load_settings(args.config) + decode = cdr_decoder() if args.format == 'cdr' else None + state = {'received': 0, 'attempts': 0, 'diagnosed': False} + connected = threading.Event() + done = threading.Event() + + client = make_client(settings, log=log) + + def on_connect(client, userdata, flags, reason_code, properties): + if reason_code == 0: + connected.set() + state['attempts'] = 0 + log.info('connected to %s, subscribing to %s', settings.address, args.topic) + client.subscribe(args.topic) + return + log.error('broker %s refused the connection: %s', settings.address, reason_text(reason_code)) + if is_auth_failure(reason_code): + log.error('check MQTT_USERNAME/MQTT_PASSWORD, the client certificate and the broker ACL') + done.set() + + def on_disconnect(client, userdata, flags, reason_code, properties): + if reason_code == 0: + log.info('disconnected from %s', settings.address) + return + state['attempts'] += 1 + log.warning('unexpected disconnect from %s: %s', settings.address, reason_text(reason_code)) + if connected.is_set(): + return + if not state['diagnosed']: + diagnose(settings, log) + state['diagnosed'] = True + if state['attempts'] >= MAX_ATTEMPTS and not done.is_set(): + log.error('giving up after %d failed connection attempts', state['attempts']) + done.set() + + def on_subscribe(client, userdata, mid, reason_codes, properties): + for rc in reason_codes: + if rc.is_failure: + log.error('broker %s denied SUBSCRIBE for %s: %s', + settings.address, args.topic, reason_text(rc)) + else: + log.info('subscribed to %s (qos %s)', args.topic, rc.value) + + def on_message(client, userdata, message): + state['received'] += 1 + if decode is None: + text = message.payload.decode('utf-8', errors='replace') + else: + try: + text = decode(message.payload) + except Exception: + log.exception('failed to deserialize %d byte payload on %s; try --format raw', + len(message.payload), message.topic) + return + print(f'[{message.topic}] {text}', flush=True) + if args.count and state['received'] >= args.count: + done.set() + + client.on_connect = on_connect + client.on_disconnect = on_disconnect + client.on_subscribe = on_subscribe + client.on_message = on_message + + log.info('connecting: %s', describe(settings)) + if settings.missing: + log.error('TLS material missing on disk: %s', ', '.join(settings.missing)) + return 1 + + try: + client.connect(settings.host, settings.port, settings.keepalive) + except socket.gaierror as exc: + log.error('cannot resolve host %r: %s', settings.host, exc) + diagnose(settings, log) + return 1 + except ssl.SSLError as exc: + log.error('TLS handshake with %s failed: %s', settings.address, exc) + diagnose(settings, log) + return 1 + except OSError as exc: + log.error('cannot reach broker at %s: %s', settings.address, exc) + diagnose(settings, log) + return 1 + + client.loop_start() + try: + if not connected.wait(CONNECT_TIMEOUT) and not done.is_set(): + log.error('no CONNACK from %s within %ss', settings.address, CONNECT_TIMEOUT) + diagnose(settings, log) + return 1 + done.wait() + except KeyboardInterrupt: + log.info('interrupted after %d message(s)', state['received']) + finally: + client.loop_stop() + client.disconnect() + + if not connected.is_set(): + return 1 + return 0 if state['received'] or not args.count else 1 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_bridge.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_bridge.sh index b2997b0b..93b770ed 100755 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_bridge.sh +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_bridge.sh @@ -1,15 +1,15 @@ #!/usr/bin/env bash -# Exercises the bridge topic mappings defined in a bridge_config.yaml. -# For each ros2_to_mqtt mapping: publishes on the MQTT topic and listens for it. -# For each mqtt_to_ros2 mapping: same in the other direction. +# Round-trips every mqtt_topic declared in a bridge_config.yaml through the broker. +# This proves reachability and ACL grants for the configured topics; it does not +# require the bridge node to be running. # -# Usage: ./mqtt_test_bridge.sh [/path/to/mqtt.env] [/path/to/bridge_config.yaml] +# Usage: ./mqtt_test_bridge.sh [/path/to/broker_config.yaml] [/path/to/bridge_config.yaml] -set -euo pipefail +set -uo pipefail SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" source "$SCRIPT_DIR/mqtt_common.sh" "${1:-}" -CONFIG="${2:-$(dirname "$SCRIPT_DIR")/config/bridge_config.yaml}" +CONFIG="${2:-${1:-$(dirname "$SCRIPT_DIR")/config/bridge_config.yaml}}" TIMEOUT=5 PASS=0 FAIL=0 @@ -24,53 +24,70 @@ echo "Broker : $MQTT_HOST:$MQTT_PORT" echo "Config : $CONFIG" echo "" +# A subscription filter cannot be published to, so '+' and '#' are replaced with +# a concrete segment that still matches the filter. +_concrete_topic() { + local topic="${1//+/probe}" + if [[ "$topic" == "#" ]]; then + echo probe + elif [[ "$topic" == */# ]]; then + echo "${topic%/#}/probe" + else + echo "$topic" + fi +} + _test_topic() { local direction="$1" - local topic="$2" + local filter="$2" + local topic + topic="$(_concrete_topic "$filter")" local payload="test-$(date +%s%N)" local tmpfile tmpfile=$(mktemp) - mosquitto_sub "${_broker_args[@]}" -t "$topic" -C 1 -W "$TIMEOUT" \ + mosquitto_sub "${_broker_args[@]}" -t "$filter" -C 1 -W "$TIMEOUT" \ > "$tmpfile" 2>/dev/null & local sub_pid=$! sleep 0.3 - mosquitto_pub "${_broker_args[@]}" -t "$topic" -m "$payload" + mosquitto_pub "${_broker_args[@]}" -t "$topic" -m "$payload" 2>/dev/null if wait "$sub_pid" 2>/dev/null && [[ "$(cat "$tmpfile")" == "$payload" ]]; then - echo " PASS [$direction] $topic" - (( PASS++ )) || true + echo " PASS [$direction] $filter" + (( PASS++ )) else - echo " FAIL [$direction] $topic" >&2 - (( FAIL++ )) || true + echo " FAIL [$direction] $filter (published to $topic)" >&2 + (( FAIL++ )) fi rm -f "$tmpfile" } -# Parse YAML with awk - extract mqtt_topic values from each section _extract_mqtt_topics() { - local section="$1" - awk " - /^${section}:/ { in_section=1; next } - /^[a-z]/ && !/^${section}:/ { in_section=0 } - in_section && /mqtt_topic:/ { gsub(/.*mqtt_topic:[[:space:]]*\"|\"/, \"\"); print } - " "$CONFIG" + awk -v section="$1" ' + $0 ~ "^" section ":" { in_section = 1; next } + /^[^[:space:]#]/ { in_section = 0 } + in_section && /mqtt_topic:/ { + sub(/^.*mqtt_topic:[[:space:]]*/, "") + sub(/[[:space:]]*#.*$/, "") + gsub(/^["\047]|["\047]$/, "") + sub(/[[:space:]]+$/, "") + if (length($0)) print + } + ' "$CONFIG" } -echo "--- ros2_to_mqtt topics ---" -while IFS= read -r topic; do - [[ -z "$topic" ]] && continue - _test_topic "ros2->mqtt" "$topic" -done < <(_extract_mqtt_topics "ros2_to_mqtt") +for section in ros2_to_mqtt mqtt_to_ros2; do + echo "--- $section topics ---" + found=0 + while IFS= read -r topic; do + [[ -z "$topic" ]] && continue + found=1 + _test_topic "$section" "$topic" + done < <(_extract_mqtt_topics "$section") + [[ "$found" -eq 0 ]] && echo " (none declared)" + echo "" +done -echo "" -echo "--- mqtt_to_ros2 topics ---" -while IFS= read -r topic; do - [[ -z "$topic" ]] && continue - _test_topic "mqtt->ros2" "$topic" -done < <(_extract_mqtt_topics "mqtt_to_ros2") - -echo "" echo "Results: $PASS passed, $FAIL failed" [[ $FAIL -eq 0 ]] diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_remote.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_remote.sh new file mode 100755 index 00000000..a6e716c5 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/mqtt_test_remote.sh @@ -0,0 +1,205 @@ +#!/usr/bin/env bash +# Answers, in order, for the broker named by config/bridge_config.yaml: +# +# 1. is the host reachable? +# 2. does TLS work? +# 3. does authentication work? +# 4. is there data on the broker? +# 5. does that data reach ROS? +# +# Each stage prints what it observed, including one received payload, so a +# failure identifies the layer that broke rather than just "connection failed". +# +# Usage: ./scripts/mqtt_test_remote.sh [/path/to/bridge_config.yaml] +# +# NMEA_WAIT seconds to wait for live NMEA data (default 30) +# NMEA_REQUIRE set to 1 to fail, rather than skip, when no NMEA data arrives +# SOLBOX_WAIT seconds to listen for live solbox notifications (default 30) +# MOCK_WAIT seconds to wait for our own published notification (default 15) + +set -uo pipefail +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PKG_ROOT="$(cd "$SCRIPT_DIR/.." && pwd)" +source "$SCRIPT_DIR/test_common.sh" + +CONFIG="${1:-$PKG_ROOT/config/bridge_config.yaml}" +NMEA_TOPIC='od_imoger/vehicles/dlr1/nmea' +NMEA_ROS_TOPIC='/imoger/vehicles/dlr1/nmea' +SOLBOX_FILTER='od_imoger/solbox/+/notifications' +SOLBOX_TOPIC='od_imoger/solbox/solbox_test/notifications' +SOLBOX_ROS_TOPIC='/imoger/solbox/notifications' +NMEA_WAIT="${NMEA_WAIT:-30}" +NMEA_REQUIRE="${NMEA_REQUIRE:-0}" +SOLBOX_WAIT="${SOLBOX_WAIT:-30}" +MOCK_WAIT="${MOCK_WAIT:-15}" + +if [[ ! -f "$CONFIG" ]]; then + echo "ERROR: config not found: $CONFIG" >&2 + exit 1 +fi + +export MQTT_BRIDGE_CONFIG="$CONFIG" +export MQTT_BRIDGE_CERT_DIR="${MQTT_BRIDGE_CERT_DIR:-$PKG_ROOT/certs}" +export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}" +export ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE:-LOCALHOST}" + +WORK_DIR="$(mktemp -d)" +trap 'stop_bridge; rm -rf "$WORK_DIR"' EXIT + +probe() { + local stage="$1" name="$2" + local out status + out="$(python3 "$SCRIPT_DIR/mqtt_probe.py" --stage "$stage" --config "$CONFIG" 2>&1)" + status=$? + echo " $out" + if [[ "$status" -eq 2 ]]; then + skip "$name" "not applicable" + return 0 + fi + record "$name" "$status" + return "$status" +} + +echo "=== Remote Broker Test ===" +echo "Config : $CONFIG" + +# Resolving the mosquitto arguments first turns missing certificates into a skip +# rather than the hard exit mqtt_common.sh would take. +if ! python3 "$SCRIPT_DIR/bridge_mqtt_args.py" "$CONFIG" > /dev/null 2> "$WORK_DIR/args.err"; then + echo "SKIP: cannot resolve broker settings from $CONFIG" + sed 's/^/ /' "$WORK_DIR/args.err" + exit 0 +fi +source "$SCRIPT_DIR/mqtt_common.sh" "$CONFIG" + +echo "Broker : $MQTT_HOST:$MQTT_PORT" +echo "User : ${MQTT_USERNAME:-}" + +banner "1. Is the host reachable?" +if ! probe tcp "host reachable ($MQTT_HOST:$MQTT_PORT)"; then + echo " nothing below this layer can be tested." >&2 + summary + exit 1 +fi + +banner "2. Is TLS working?" +if ! probe tls "tls handshake"; then + echo " nothing below this layer can be tested." >&2 + summary + exit 1 +fi + +banner "3. Does authentication work?" +if [[ -z "${MQTT_USERNAME:-}" || -z "${MQTT_PASSWORD:-}" ]]; then + skip "broker authentication" "no credentials in the environment or .mqtt_secrets.env" + summary + exit 0 +fi +if ! probe auth "broker authentication"; then + summary + exit 1 +fi + +banner "4. Is there data on the broker?" +echo " waiting up to ${NMEA_WAIT}s for $NMEA_TOPIC" +timeout $((NMEA_WAIT + 10)) mosquitto_sub "${_broker_args[@]}" \ + -t "$NMEA_TOPIC" -C 1 -W "$NMEA_WAIT" \ + > "$WORK_DIR/nmea.log" 2> "$WORK_DIR/nmea.err" +if [[ -s "$WORK_DIR/nmea.log" ]]; then + show_payload "nmea" "$WORK_DIR/nmea.log" + record "live data on $NMEA_TOPIC" 0 +elif [[ "$NMEA_REQUIRE" == "1" ]]; then + record "live data on $NMEA_TOPIC" 1 "$WORK_DIR/nmea.err" +else + skip "live data on $NMEA_TOPIC" "silent for ${NMEA_WAIT}s, vehicle is probably offline" +fi + +# The solbox topic carries a live mock publisher, so a subscriber sees other +# people's traffic. Every check below correlates on action_id rather than +# taking whichever message happens to arrive first. +echo " listening ${SOLBOX_WAIT}s on $SOLBOX_FILTER" +timeout $((SOLBOX_WAIT + 5)) mosquitto_sub "${_broker_args[@]}" \ + -t "$SOLBOX_FILTER" -W "$SOLBOX_WAIT" \ + > "$WORK_DIR/solbox_live.log" 2> "$WORK_DIR/solbox_live.err" +if show_payload "solbox" "$WORK_DIR/solbox_live.log"; then + record "live data on $SOLBOX_FILTER" 0 + head -n 1 "$WORK_DIR/solbox_live.log" > "$WORK_DIR/solbox_first.json" + python3 "$SCRIPT_DIR/dimos_message.py" --validate "$WORK_DIR/solbox_first.json" \ + > "$WORK_DIR/validate.log" 2>&1 + status=$? + sed 's/^/ /' "$WORK_DIR/validate.log" + record "notification structure is valid" "$status" "$WORK_DIR/validate.log" +else + skip "live data on $SOLBOX_FILTER" "silent for ${SOLBOX_WAIT}s" + skip "notification structure is valid" "no message to validate" +fi + +ACTION_ID=$(( (RANDOM << 15 | RANDOM) % 900000000 + 100000000 )) +python3 "$SCRIPT_DIR/dimos_message.py" --generate --action-id "$ACTION_ID" \ + > "$WORK_DIR/mock.json" +echo " publishing a notification to $SOLBOX_TOPIC (action_id $ACTION_ID)" + +timeout $((MOCK_WAIT + 5)) mosquitto_sub "${_broker_args[@]}" \ + -t "$SOLBOX_FILTER" -W "$MOCK_WAIT" > "$WORK_DIR/solbox.log" 2>&1 & +sub_pid=$! +sleep 2 +mosquitto_pub "${_broker_args[@]}" -t "$SOLBOX_TOPIC" -f "$WORK_DIR/mock.json" \ + 2> "$WORK_DIR/solbox_pub.err" +pub_status=$? +wait "$sub_pid" 2>/dev/null + +if [[ "$pub_status" -ne 0 ]]; then + record "publish to $SOLBOX_TOPIC" 1 "$WORK_DIR/solbox_pub.err" +else + record "publish to $SOLBOX_TOPIC" 0 + grep -q "$ACTION_ID" "$WORK_DIR/solbox.log" + record "own notification returns on $SOLBOX_FILTER" $? "$WORK_DIR/solbox.log" +fi + +banner "5. Does the data reach ROS?" +start_bridge "$CONFIG" "$WORK_DIR/bridge.log" 40 +bridge_status=$? +if [[ "$bridge_status" -eq 2 ]]; then + skip "data reaches ROS" "ros2 not on PATH" +elif [[ "$bridge_status" -ne 0 ]]; then + record "bridge node connects to $MQTT_HOST:$MQTT_PORT" 1 "$WORK_DIR/bridge.log" +else + record "bridge node connects to $MQTT_HOST:$MQTT_PORT" 0 + + if [[ -s "$WORK_DIR/nmea.log" ]]; then + timeout $((NMEA_WAIT + 10)) ros2 topic echo --once --full-length \ + "$NMEA_ROS_TOPIC" std_msgs/msg/String > "$WORK_DIR/nmea_ros.log" 2>&1 + if show_payload "nmea (ros)" "$WORK_DIR/nmea_ros.log"; then + record "live NMEA reaches $NMEA_ROS_TOPIC" 0 + else + record "live NMEA reaches $NMEA_ROS_TOPIC" 1 "$WORK_DIR/nmea_ros.log" + fi + else + skip "live NMEA reaches $NMEA_ROS_TOPIC" "no NMEA data was seen on the broker" + fi + + # --full-length matters: ros2 topic echo abbreviates long strings by + # default, which truncates the JSON before action_id is visible. + ACTION_ID=$(( (RANDOM << 15 | RANDOM) % 900000000 + 100000000 )) + python3 "$SCRIPT_DIR/dimos_message.py" --generate --action-id "$ACTION_ID" \ + > "$WORK_DIR/mock_ros.json" + timeout $((MOCK_WAIT + 10)) ros2 topic echo --full-length \ + "$SOLBOX_ROS_TOPIC" std_msgs/msg/String > "$WORK_DIR/ros.log" 2>&1 & + echo_pid=$! + sleep 3 + for _ in 1 2 3 4 5; do + mosquitto_pub "${_broker_args[@]}" -t "$SOLBOX_TOPIC" -f "$WORK_DIR/mock_ros.json" 2>/dev/null + sleep 2 + grep -q "$ACTION_ID" "$WORK_DIR/ros.log" && break + done + kill "$echo_pid" 2>/dev/null + wait "$echo_pid" 2>/dev/null + grep -q "$ACTION_ID" "$WORK_DIR/ros.log" + ros_status=$? + grep -m1 "$ACTION_ID" "$WORK_DIR/ros.log" > "$WORK_DIR/ros_match.log" 2>/dev/null + show_payload "solbox (ros)" "$WORK_DIR/ros_match.log" || true + record "own notification reaches $SOLBOX_ROS_TOPIC (action_id $ACTION_ID)" "$ros_status" "$WORK_DIR/ros.log" + stop_bridge +fi + +summary diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/publish_vehicle_state.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/publish_vehicle_state.py old mode 100644 new mode 100755 diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/run_tests.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/run_tests.sh new file mode 100755 index 00000000..31f26d2f --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/run_tests.sh @@ -0,0 +1,197 @@ +#!/usr/bin/env bash +# End-to-end test run against a throwaway local broker. +# +# Runs the local suite against a throwaway broker, then the remote suite against +# the broker in config/bridge_config.yaml. +# +# Usage: ./scripts/run_tests.sh [--no-broker] [--config PATH] [--no-remote] +# +# --no-broker skip starting mosquitto and use the broker the config names +# --config test against a different bridge config (implies a real broker) +# --no-remote skip the remote broker suite + +set -uo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PKG_ROOT="$(cd "$SCRIPT_DIR/.." && pwd)" + +source "$SCRIPT_DIR/test_common.sh" + +START_BROKER=1 +RUN_REMOTE=1 +CONFIG="$PKG_ROOT/test/bridge_config.test.yaml" +REMOTE_CONFIG="${REMOTE_CONFIG:-$PKG_ROOT/config/bridge_config.yaml}" + +while [[ $# -gt 0 ]]; do + case "$1" in + --no-broker) START_BROKER=0; shift ;; + --config) CONFIG="$2"; START_BROKER=0; shift 2 ;; + --remote) RUN_REMOTE=1; shift ;; + --no-remote) RUN_REMOTE=0; shift ;; + *) echo "unknown argument: $1" >&2; exit 2 ;; + esac +done + +if [[ ! -f "$CONFIG" ]]; then + echo "ERROR: config not found: $CONFIG" >&2 + exit 1 +fi + +# Deliberately not inherited: the ambient value points at the production broker. +export MQTT_BRIDGE_CONFIG="$CONFIG" +export MQTT_BRIDGE_CERT_DIR="${MQTT_BRIDGE_CERT_DIR:-$PKG_ROOT/certs}" +export RMW_IMPLEMENTATION="${RMW_IMPLEMENTATION:-rmw_fastrtps_cpp}" +export ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE:-LOCALHOST}" +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-42}" +export PYTHONUNBUFFERED=1 +unset MQTT_HOST MQTT_PORT MQTT_TLS MQTT_USERNAME MQTT_PASSWORD \ + MQTT_CA_CERT MQTT_CLIENT_CERT MQTT_CLIENT_KEY + +WORK_DIR="$(mktemp -d)" +BROKER_PID="" + +cleanup() { + stop_bridge + [[ -n "$BROKER_PID" ]] && kill "$BROKER_PID" 2>/dev/null + wait 2>/dev/null + rm -rf "$WORK_DIR" +} +trap cleanup EXIT + +start_broker() { + cat > "$WORK_DIR/mosquitto.conf" <<'EOF' +listener 1883 127.0.0.1 +allow_anonymous true +persistence false +EOF + mosquitto -c "$WORK_DIR/mosquitto.conf" > "$WORK_DIR/mosquitto.log" 2>&1 & + BROKER_PID=$! + if ! wait_for_port 127.0.0.1 1883; then + echo "ERROR: local mosquitto did not come up" >&2 + cat "$WORK_DIR/mosquitto.log" >&2 + exit 1 + fi + echo "local broker running on 127.0.0.1:1883 (pid $BROKER_PID)" +} + +banner "Unit tests" +( cd "$PKG_ROOT" && python3 -m unittest discover -s test -t "$PKG_ROOT" -v ) \ + > "$WORK_DIR/unit.log" 2>&1 +record "unit tests" $? "$WORK_DIR/unit.log" + +if [[ "$START_BROKER" -eq 1 ]]; then + banner "Local broker" + start_broker +fi + +banner "Broker reachability" +"$SCRIPT_DIR/mqtt_check_broker.sh" "$MQTT_BRIDGE_CONFIG" > "$WORK_DIR/check.log" 2>&1 +record "mqtt_check_broker.sh" $? "$WORK_DIR/check.log" + +banner "mosquitto round trip" +"$SCRIPT_DIR/mqtt_test_pubsub.sh" "$MQTT_BRIDGE_CONFIG" > "$WORK_DIR/pubsub.log" 2>&1 +record "mqtt_test_pubsub.sh" $? "$WORK_DIR/pubsub.log" + +banner "Python script round trip" +expected="script-round-trip-$$" +python3 "$SCRIPT_DIR/mqtt_subscribe.py" test/scripts --count 1 --format raw \ + > "$WORK_DIR/sub.log" 2>&1 & +sub_pid=$! +sleep 3 +python3 "$SCRIPT_DIR/mqtt_publish.py" test/scripts --count 3 --interval 0.5 \ + --format raw --message "$expected" > "$WORK_DIR/pub.log" 2>&1 +pub_status=$? +wait "$sub_pid" 2>/dev/null +if [[ "$pub_status" -eq 0 ]] && grep -q "$expected" "$WORK_DIR/sub.log"; then + record "mqtt_publish.py -> mqtt_subscribe.py" 0 +else + cat "$WORK_DIR/pub.log" >> "$WORK_DIR/sub.log" + record "mqtt_publish.py -> mqtt_subscribe.py" 1 "$WORK_DIR/sub.log" +fi + +banner "Diagnostics on an unreachable broker" +MQTT_HOST=127.0.0.1 MQTT_PORT=1 timeout 30 python3 "$SCRIPT_DIR/mqtt_subscribe.py" \ + test/unreachable --format raw > "$WORK_DIR/unreachable.log" 2>&1 +status=$? +if [[ "$status" -eq 1 ]] && grep -q '127.0.0.1:1' "$WORK_DIR/unreachable.log"; then + record "connection failure reports host and port" 0 +else + record "connection failure reports host and port" 1 "$WORK_DIR/unreachable.log" +fi + +banner "Bridge node" +bridge_status=3 +if [[ "$START_BROKER" -eq 0 ]]; then + skip "bridge node round trip" "needs the local broker" +else + start_bridge "$MQTT_BRIDGE_CONFIG" "$WORK_DIR/bridge.log" + bridge_status=$? +fi + +if [[ "$bridge_status" -eq 2 ]]; then + skip "bridge node round trip" "ros2 not on PATH" +elif [[ "$bridge_status" -eq 0 ]]; then + record "bridge node connects" 0 + + payload="ros-to-mqtt-$$" + mosquitto_sub -h 127.0.0.1 -p 1883 -t test/outbound -C 1 -W 15 \ + > "$WORK_DIR/r2m.log" 2>&1 & + sub_pid=$! + sleep 1 + ros2 topic pub -r 2 /test/outbound std_msgs/msg/String "{data: '$payload'}" \ + > "$WORK_DIR/r2m_pub.log" 2>&1 & + pub_pid=$! + wait "$sub_pid" 2>/dev/null + kill "$pub_pid" 2>/dev/null + wait "$pub_pid" 2>/dev/null + grep -q "$payload" "$WORK_DIR/r2m.log" + record "ros2 -> mqtt (/test/outbound)" $? "$WORK_DIR/r2m.log" + + timeout 20 ros2 topic echo --once /test/inbound std_msgs/msg/String \ + > "$WORK_DIR/m2r.log" 2>&1 & + echo_pid=$! + sleep 3 + payload="mqtt-to-ros-$$" + for _ in 1 2 3; do + mosquitto_pub -h 127.0.0.1 -p 1883 -t test/inbound -m "$payload" + sleep 1 + kill -0 "$echo_pid" 2>/dev/null || break + done + wait "$echo_pid" 2>/dev/null + grep -q "$payload" "$WORK_DIR/m2r.log" + record "mqtt -> ros2 (/test/inbound)" $? "$WORK_DIR/m2r.log" + + timeout 20 ros2 topic echo --once /test/wildcard std_msgs/msg/String \ + > "$WORK_DIR/wild.log" 2>&1 & + echo_pid=$! + sleep 3 + payload="wildcard-$$" + for _ in 1 2 3; do + mosquitto_pub -h 127.0.0.1 -p 1883 -t test/wildcard/unit0/data -m "$payload" + sleep 1 + kill -0 "$echo_pid" 2>/dev/null || break + done + wait "$echo_pid" 2>/dev/null + grep -q "$payload" "$WORK_DIR/wild.log" + record "mqtt wildcard -> ros2 (/test/wildcard)" $? "$WORK_DIR/wild.log" + + stop_bridge +elif [[ "$bridge_status" -eq 1 ]]; then + record "bridge node connects" 1 "$WORK_DIR/bridge.log" +fi + +banner "Configured topic loopback" +"$SCRIPT_DIR/mqtt_test_bridge.sh" "$MQTT_BRIDGE_CONFIG" "$MQTT_BRIDGE_CONFIG" \ + > "$WORK_DIR/topics.log" 2>&1 +record "mqtt_test_bridge.sh" $? "$WORK_DIR/topics.log" + +if [[ "$RUN_REMOTE" -eq 1 ]]; then + printf '\n########## Remote broker suite ##########\n' + RESULTS_FILE="$WORK_DIR/remote.results" "$SCRIPT_DIR/mqtt_test_remote.sh" "$REMOTE_CONFIG" + absorb_results "$WORK_DIR/remote.results" + printf '########## End remote broker suite ##########\n' +else + skip "remote broker suite" "--no-remote" +fi + +summary diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/start_local_mqtt_broker.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/start_local_mqtt_broker.sh similarity index 100% rename from ros2_workspace/src/adore_interfaces/mqtt_message_bridge/start_local_mqtt_broker.sh rename to ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/start_local_mqtt_broker.sh diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/stop_local_mqtt_broker.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/stop_local_mqtt_broker.sh similarity index 100% rename from ros2_workspace/src/adore_interfaces/mqtt_message_bridge/stop_local_mqtt_broker.sh rename to ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/stop_local_mqtt_broker.sh diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/test_common.sh b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/test_common.sh new file mode 100644 index 00000000..ab192008 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/scripts/test_common.sh @@ -0,0 +1,109 @@ +# Sourced by run_tests.sh and mqtt_test_remote.sh. Not executable on its own. + +PASS=0 +FAIL=0 +SKIPPED=0 +FAILED_CASES=() +BRIDGE_PID="" + +banner() { printf '\n=== %s ===\n' "$1"; } + +record() { + local name="$1" status="$2" log="${3:-}" + if [[ "$status" -eq 0 ]]; then + echo "PASS: $name" + PASS=$((PASS + 1)) + else + echo "FAIL: $name" >&2 + [[ -n "$log" && -s "$log" ]] && sed 's/^/ /' "$log" >&2 + FAILED_CASES+=("$name") + FAIL=$((FAIL + 1)) + fi +} + +skip() { + echo "SKIP: $1 ($2)" + SKIPPED=$((SKIPPED + 1)) +} + +# Prints the first line of a payload, clipped, so a firehose topic cannot flood +# the test output. +show_payload() { + local label="$1" file="$2" + if [[ ! -s "$file" ]]; then + echo " $label: " + return 1 + fi + echo " $label: $(head -c 300 "$file" | head -n 1)" +} + +wait_for_port() { + local host="$1" port="$2" limit="${3:-15}" + local deadline=$((SECONDS + limit)) + while (( SECONDS < deadline )); do + nc -z -w1 "$host" "$port" 2>/dev/null && return 0 + sleep 0.2 + done + return 1 +} + +start_bridge() { + local config="$1" logfile="$2" limit="${3:-30}" + if ! command -v ros2 >/dev/null 2>&1; then + return 2 + fi + ros2 run mqtt_message_bridge bridge_node \ + --ros-args -p "config_path:=$config" > "$logfile" 2>&1 & + BRIDGE_PID=$! + local deadline=$((SECONDS + limit)) + while (( SECONDS < deadline )); do + grep -q 'Connected to MQTT broker' "$logfile" && break + kill -0 "$BRIDGE_PID" 2>/dev/null || break + sleep 0.5 + done + if ! grep -q 'Connected to MQTT broker' "$logfile"; then + return 1 + fi + sleep 2 + return 0 +} + +stop_bridge() { + [[ -z "$BRIDGE_PID" ]] && return 0 + kill "$BRIDGE_PID" 2>/dev/null + wait "$BRIDGE_PID" 2>/dev/null + BRIDGE_PID="" +} + +# Lets a nested suite fold its counts into the parent's summary. +export_results() { + [[ -n "${RESULTS_FILE:-}" ]] || return 0 + { + printf '%s %s %s\n' "$PASS" "$FAIL" "$SKIPPED" + [[ ${#FAILED_CASES[@]} -gt 0 ]] && printf '%s\n' "${FAILED_CASES[@]}" + } > "$RESULTS_FILE" +} + +absorb_results() { + local file="$1" + [[ -s "$file" ]] || return 0 + local p f s + read -r p f s < "$file" + PASS=$((PASS + p)) + FAIL=$((FAIL + f)) + SKIPPED=$((SKIPPED + s)) + while IFS= read -r case_name; do + [[ -n "$case_name" ]] && FAILED_CASES+=("$case_name") + done < <(tail -n +2 "$file") +} + +summary() { + export_results + banner "Summary" + echo "$PASS passed, $FAIL failed, $SKIPPED skipped" + if (( FAIL > 0 )); then + printf 'failed: %s\n' "${FAILED_CASES[*]}" >&2 + return 1 + fi + return 0 +} diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/__init__.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/bridge_config.test.yaml b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/bridge_config.test.yaml new file mode 100644 index 00000000..86eb6ebf --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/bridge_config.test.yaml @@ -0,0 +1,48 @@ +mqtt: + host: 127.0.0.1 + port: 1883 + keepalive: 60 + transport: tcp + protocol: mqtt + qos: 1 + + tls: + enabled: false + + reconnect_delay: 1 + reconnect_max_delay: 5 + +ros2_to_mqtt: + - ros_topic: /test/outbound + mqtt_topic: test/outbound + msg_type: std_msgs/msg/String + format: raw + qos_depth: 10 + qos_reliability: reliable + qos_durability: volatile + + - ros_topic: /test/outbound_cdr + mqtt_topic: test/outbound_cdr + msg_type: std_msgs/msg/String + format: cdr + qos_depth: 10 + qos_reliability: reliable + qos_durability: volatile + +mqtt_to_ros2: + - mqtt_topic: test/inbound + ros_topic: /test/inbound + msg_type: std_msgs/msg/String + format: raw + mqtt_qos: 1 + qos_depth: 10 + qos_reliability: reliable + qos_durability: transient_local + + - mqtt_topic: test/wildcard/+/data + ros_topic: /test/wildcard + msg_type: std_msgs/msg/String + format: raw + qos_depth: 10 + qos_reliability: reliable + qos_durability: transient_local diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_broker_settings.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_broker_settings.py new file mode 100644 index 00000000..68ac852c --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_broker_settings.py @@ -0,0 +1,159 @@ +import os +import sys +import tempfile +import unittest + +sys.path.insert(0, os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'scripts')) + +from bridge_mqtt_args import broker_args +from mqtt_broker import as_bool, load_settings, port_speaks_tls, reason_text + +BASE_CONFIG = """ +mqtt: + host: config-host + port: 1884 + keepalive: 30 + auth: + username_env: MQTT_USERNAME + password_env: MQTT_PASSWORD + tls: + enabled: false +""" + +TLS_CONFIG = """ +mqtt: + host: secure-host + port: 8883 + env_file: {env_file} + auth: + username_env: MQTT_USERNAME + password_env: MQTT_PASSWORD + tls: + enabled: true + ca_certs: broker.crt + certfile: client.crt + keyfile: client.key + insecure: true +""" + + +class SettingsTestCase(unittest.TestCase): + def setUp(self): + self.tmp = tempfile.TemporaryDirectory() + self.addCleanup(self.tmp.cleanup) + self.saved_env = {k: os.environ.get(k) for k in ( + 'MQTT_HOST', 'MQTT_PORT', 'MQTT_TLS', 'MQTT_USERNAME', 'MQTT_PASSWORD', + 'MQTT_BRIDGE_CERT_DIR', 'MQTT_BRIDGE_CONFIG', 'MQTT_KEEPALIVE', + )} + for key in self.saved_env: + os.environ.pop(key, None) + self.addCleanup(self._restore_env) + + def _restore_env(self): + for key, value in self.saved_env.items(): + if value is None: + os.environ.pop(key, None) + else: + os.environ[key] = value + + def write(self, name, text): + path = os.path.join(self.tmp.name, name) + with open(path, 'w') as f: + f.write(text) + return path + + def test_values_come_from_config_when_env_is_unset(self): + settings = load_settings(self.write('a.yaml', BASE_CONFIG)) + self.assertEqual(settings.host, 'config-host') + self.assertEqual(settings.port, 1884) + self.assertEqual(settings.keepalive, 30) + self.assertFalse(settings.tls) + + def test_environment_overrides_config(self): + os.environ['MQTT_HOST'] = 'env-host' + os.environ['MQTT_PORT'] = '9999' + settings = load_settings(self.write('b.yaml', BASE_CONFIG)) + self.assertEqual(settings.address, 'env-host:9999') + + def test_env_file_does_not_override_real_environment(self): + env_file = self.write('secrets.env', 'MQTT_HOST=file-host\nMQTT_USERNAME=file-user\n') + os.environ['MQTT_HOST'] = 'env-host' + settings = load_settings(self.write('c.yaml', TLS_CONFIG.format(env_file=env_file))) + self.assertEqual(settings.host, 'env-host') + self.assertEqual(settings.username, 'file-user') + + def test_relative_tls_paths_resolve_against_cert_dir(self): + cert_dir = os.path.join(self.tmp.name, 'certs') + os.makedirs(cert_dir) + for name in ('broker.crt', 'client.crt', 'client.key'): + open(os.path.join(cert_dir, name), 'w').close() + os.environ['MQTT_BRIDGE_CERT_DIR'] = cert_dir + env_file = self.write('empty.env', '') + settings = load_settings(self.write('d.yaml', TLS_CONFIG.format(env_file=env_file))) + self.assertTrue(settings.tls) + self.assertEqual(settings.ca_certs, os.path.join(cert_dir, 'broker.crt')) + self.assertEqual(settings.missing, []) + + def test_missing_tls_material_is_reported(self): + os.environ['MQTT_BRIDGE_CERT_DIR'] = os.path.join(self.tmp.name, 'nope') + env_file = self.write('empty2.env', '') + settings = load_settings(self.write('e.yaml', TLS_CONFIG.format(env_file=env_file))) + self.assertEqual(len(settings.missing), 3) + + def test_mosquitto_args_carry_credentials_and_tls(self): + cert_dir = os.path.join(self.tmp.name, 'certs2') + os.makedirs(cert_dir) + for name in ('broker.crt', 'client.crt', 'client.key'): + open(os.path.join(cert_dir, name), 'w').close() + os.environ['MQTT_BRIDGE_CERT_DIR'] = cert_dir + os.environ['MQTT_PASSWORD'] = 'pw' + env_file = self.write('user.env', 'MQTT_USERNAME=alice\n') + args = broker_args(load_settings(self.write('f.yaml', TLS_CONFIG.format(env_file=env_file)))) + self.assertEqual(args[:4], ['-h', 'secure-host', '-p', '8883']) + self.assertIn('--cafile', args) + self.assertIn('--insecure', args) + self.assertEqual(args[args.index('-u') + 1], 'alice') + self.assertEqual(args[args.index('-P') + 1], 'pw') + + def test_missing_config_falls_back_to_defaults(self): + settings = load_settings(os.path.join(self.tmp.name, 'absent.yaml')) + self.assertEqual(settings.address, 'localhost:1883') + self.assertIsNone(settings.config_path) + + def test_as_bool_accepts_string_flags(self): + self.assertTrue(as_bool('1')) + self.assertTrue(as_bool('TRUE')) + self.assertFalse(as_bool('0')) + self.assertFalse(as_bool('')) + + def test_reason_text_handles_paho_reason_codes(self): + from paho.mqtt.packettypes import PacketTypes + from paho.mqtt.reasoncodes import ReasonCode + text = reason_text(ReasonCode(PacketTypes.CONNACK, 'Not authorized')) + self.assertIn('Not authorized', text) + self.assertIn('135', text) + self.assertIn('7', reason_text(7)) + + +class TlsProbeTestCase(unittest.TestCase): + def test_plaintext_listener_is_not_tls(self): + import socket + import threading + server = socket.socket() + server.bind(('127.0.0.1', 0)) + server.listen(1) + self.addCleanup(server.close) + threading.Thread(target=lambda: server.accept(), daemon=True).start() + self.assertIs(port_speaks_tls('127.0.0.1', server.getsockname()[1], timeout=2), False) + + def test_closed_port_is_unreachable(self): + import socket + probe = socket.socket() + probe.bind(('127.0.0.1', 0)) + port = probe.getsockname()[1] + probe.close() + self.assertIsNone(port_speaks_tls('127.0.0.1', port, timeout=2)) + + +if __name__ == '__main__': + unittest.main() diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_dimos_message.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_dimos_message.py new file mode 100644 index 00000000..a425befc --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_dimos_message.py @@ -0,0 +1,104 @@ +import copy +import json +import os +import sys +import unittest + +sys.path.insert(0, os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), 'scripts')) + +from dimos_message import build, validate + +# Observed on od_imoger/solbox/+/notifications. +LIVE_SAMPLES = [ + json.loads(''' + { + "header": {"protocol_ver": "0.1.0", "msg_type": "warning", "origin_station_id": "solbox1"}, + "payload": { + "management": { + "action_id": 73501, + "detection_time": "2026-08-10 09:30:45", + "reference_time": "2026-08-10 09:30:45", + "termination": 1, + "event_position": {"lat": 46.852173, "lon": 12.460039, "alt": 637.3}, + "awareness_distance": 139.5, + "transmission_interval": 2, + "station_type": 10 + }, + "situation": {"event_type": {"cause_code": "pos"}, "road_type": 2} + }, + "alacarte": { + "warning": {"continuity": {"r_hpl": 146.6}}, + "alert": {"accuracy": {"hpe": 11.5}, "integrity": {"hpl": 98.6}} + } + } + '''), + json.loads(''' + { + "header": {"protocol_ver": "0.1.0", "msg_type": "warning", "origin_station_id": "solbox1"}, + "payload": { + "management": { + "action_id": 91422, + "detection_time": "2026-08-10 09:41:02", + "reference_time": "2026-08-10 09:41:02", + "termination": 0, + "event_position": {"lat": 46.85, "lon": 12.46, "alt": 640.1}, + "awareness_distance": 172.9, + "transmission_interval": 10, + "station_type": 10 + }, + "situation": {"event_type": {"cause_code": "integrity"}, "road_type": 5} + }, + "alacarte": {"warning": {"continuity": {"r_hpl": 150.0}}} + } + '''), +] + + +class ValidatorTestCase(unittest.TestCase): + def test_live_samples_are_accepted(self): + for index, sample in enumerate(LIVE_SAMPLES): + self.assertEqual(validate(sample), [], f'sample {index}') + + def test_values_are_not_constrained(self): + message = copy.deepcopy(LIVE_SAMPLES[0]) + message['payload']['situation']['event_type']['cause_code'] = 'anything' + message['payload']['situation']['road_type'] = 99 + message['payload']['management']['station_type'] = 3 + message['payload']['management']['transmission_interval'] = 17 + message['payload']['management']['awareness_distance'] = 12.5 + message['header']['msg_type'] = 'something_new' + self.assertEqual(validate(message), []) + + def test_generated_messages_are_accepted(self): + for msg_type in ('warning', 'alert'): + self.assertEqual(validate(build(msg_type)), [], msg_type) + + def test_generated_action_id_is_used_verbatim(self): + self.assertEqual(build(action_id=4242)['payload']['management']['action_id'], 4242) + + def test_missing_field_is_an_error(self): + broken = copy.deepcopy(LIVE_SAMPLES[0]) + del broken['payload']['management']['action_id'] + self.assertTrue(any('action_id' in e for e in validate(broken))) + + def test_wrong_type_is_an_error(self): + broken = copy.deepcopy(LIVE_SAMPLES[0]) + broken['payload']['management']['event_position']['lat'] = 'north' + self.assertTrue(any('lat' in e for e in validate(broken))) + + def test_booleans_are_not_accepted_as_numbers(self): + broken = copy.deepcopy(LIVE_SAMPLES[0]) + broken['payload']['management']['termination'] = True + self.assertTrue(any('termination' in e for e in validate(broken))) + + def test_timestamp_format_is_checked(self): + broken = copy.deepcopy(LIVE_SAMPLES[0]) + broken['payload']['management']['detection_time'] = '2025-07-23T13:00:00Z' + self.assertTrue(any('detection_time' in e for e in validate(broken))) + + def test_non_object_payload_is_rejected(self): + self.assertEqual(len(validate([1, 2, 3])), 1) + + +if __name__ == '__main__': + unittest.main() diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_utils.py b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_utils.py new file mode 100644 index 00000000..cb933629 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/test/test_utils.py @@ -0,0 +1,75 @@ +import json +import os +import tempfile +import unittest + +try: + from std_msgs.msg import String +except ImportError as exc: + raise unittest.SkipTest(f'ROS 2 environment not sourced: {exc}') + +from mqtt_message_bridge.utils import ( + bytes_to_msg, + cdr_json_to_msg, + ensure_self_signed_cert, + json_to_msg, + load_msg_type, + msg_to_bytes, + msg_to_cdr_json, + msg_to_json, + raw_to_str_msg, + str_msg_to_raw, +) + +STR_TYPE = 'std_msgs/msg/String' + + +class LoadMsgTypeTestCase(unittest.TestCase): + def test_resolves_a_valid_type(self): + self.assertIs(load_msg_type(STR_TYPE), String) + + def test_rejects_a_malformed_type(self): + with self.assertRaises(ValueError): + load_msg_type('std_msgs/String') + + +class SerializationTestCase(unittest.TestCase): + def setUp(self): + self.msg = String(data='hello bridge') + + def test_cdr_round_trip(self): + self.assertEqual(bytes_to_msg(msg_to_bytes(self.msg), String).data, self.msg.data) + + def test_json_round_trip_strips_metadata(self): + payload = msg_to_json(self.msg, STR_TYPE) + self.assertEqual(json.loads(payload.decode())['datatype'], STR_TYPE) + self.assertEqual(json_to_msg(payload, String).data, self.msg.data) + + def test_cdr_json_is_readable_as_a_plain_string_message(self): + payload = msg_to_cdr_json(self.msg, STR_TYPE) + wrapper = bytes_to_msg(payload, String) + self.assertEqual(json.loads(wrapper.data)['data'], self.msg.data) + self.assertEqual(cdr_json_to_msg(payload, String).data, self.msg.data) + + def test_raw_round_trip_is_byte_exact(self): + self.assertEqual(str_msg_to_raw(self.msg), b'hello bridge') + self.assertEqual(raw_to_str_msg(b'hello bridge').data, self.msg.data) + + def test_raw_decode_tolerates_invalid_utf8(self): + self.assertIsInstance(raw_to_str_msg(b'\xff\xfe').data, str) + + +class CertGenerationTestCase(unittest.TestCase): + def test_generates_once_and_reuses(self): + with tempfile.TemporaryDirectory() as tmp: + store = os.path.join(tmp, 'store') + cert, key = ensure_self_signed_cert(store, 'test_cn', 1) + self.assertTrue(os.path.exists(cert) and os.path.exists(key)) + self.assertEqual(os.stat(key).st_mode & 0o777, 0o600) + mtime = os.stat(cert).st_mtime_ns + self.assertEqual(ensure_self_signed_cert(store, 'test_cn', 1), (cert, key)) + self.assertEqual(os.stat(cert).st_mtime_ns, mtime) + + +if __name__ == '__main__': + unittest.main() diff --git a/setup.sh b/setup.sh index 3e641c3f..91179315 100644 --- a/setup.sh +++ b/setup.sh @@ -60,5 +60,5 @@ bash ${SCRIPT_DIRECTORY}/tools/adore_api/adore_api.sh bash ${SCRIPT_DIRECTORY}/tools/start_zenoh_router.sh bash ${SCRIPT_DIRECTORY}/tools/start_zenoh_bridge.sh bash ${SCRIPT_DIRECTORY}/tools/start_sumo_bridge.sh -bash ${SCRIPT_DIRECTORY}/tools/start_mqtt_broker.sh +bash ${SCRIPT_DIRECTORY}/tools/start_grpc_message_bridge.sh bash ${SCRIPT_DIRECTORY}/tools/start_mqtt_bridge.sh diff --git a/tools/start_mqtt_bridge.sh b/tools/start_mqtt_bridge.sh index 9a9503eb..380bedb8 100644 --- a/tools/start_mqtt_bridge.sh +++ b/tools/start_mqtt_bridge.sh @@ -10,6 +10,7 @@ if [ -f "${ROS2_WORKSPACE_DIRECTORY}/install/local_setup.bash" ]; then source "${ROS2_WORKSPACE_DIRECTORY}/install/local_setup.bash" fi +PACKAGE_DIRECTORY="${MQTT_BRIDGE_PACKAGE_DIR:-${ROS2_WORKSPACE_DIRECTORY}/src/adore_interfaces/mqtt_message_bridge}" LOG_DIR="${SOURCE_DIRECTORY}/.log/mqtt" PIDFILE="${LOG_DIR}/mqtt_bridge.pid" LOGFILE="${LOG_DIR}/mqtt_bridge.log" @@ -25,7 +26,34 @@ if [ -f "${PIDFILE}" ] && kill -0 "$(cat "${PIDFILE}")" 2>/dev/null; then exit 0 fi +# The config moved from /bridge_config.yaml to /config/. +DEFAULT_CONFIG="${PACKAGE_DIRECTORY}/config/bridge_config.yaml" +if [ -z "${MQTT_BRIDGE_CONFIG:-}" ]; then + MQTT_BRIDGE_CONFIG="${DEFAULT_CONFIG}" +elif [ ! -f "${MQTT_BRIDGE_CONFIG}" ]; then + RELOCATED="$(dirname "${MQTT_BRIDGE_CONFIG}")/config/$(basename "${MQTT_BRIDGE_CONFIG}")" + if [ -f "${RELOCATED}" ]; then + echo "Note: MQTT_BRIDGE_CONFIG points at the old location; using ${RELOCATED}" + echo " update adore.env to avoid this fallback." + MQTT_BRIDGE_CONFIG="${RELOCATED}" + fi +fi +export MQTT_BRIDGE_CONFIG + +if [ ! -f "${MQTT_BRIDGE_CONFIG}" ]; then + echo "Error: bridge config not found: ${MQTT_BRIDGE_CONFIG}" + echo " expected ${DEFAULT_CONFIG}" + echo " set MQTT_BRIDGE_CONFIG or MQTT_BRIDGE_PACKAGE_DIR in adore.env" + exit 1 +fi + +# Certificates and .mqtt_secrets.env live in the source tree and are read +# relative to it, so both must be pinned when launching from the install space. +export MQTT_BRIDGE_CERT_DIR="${MQTT_BRIDGE_CERT_DIR:-${PACKAGE_DIRECTORY}/certs}" + echo "Starting mqtt_message_bridge -> ${LOGFILE}" +echo " config ${MQTT_BRIDGE_CONFIG}" +echo " certs ${MQTT_BRIDGE_CERT_DIR}" export RMW_IMPLEMENTATION=rmw_fastrtps_cpp ros2 launch mqtt_message_bridge bridge.launch.py \ config_path:="${MQTT_BRIDGE_CONFIG}" \ @@ -33,3 +61,14 @@ ros2 launch mqtt_message_bridge bridge.launch.py \ BRIDGE_PID=$! echo $BRIDGE_PID > "${PIDFILE}" echo " pid ${BRIDGE_PID}" + +# The node exits immediately on a bad config or an unreachable broker, so a +# silent background launch would otherwise look like success. +sleep 3 +if ! kill -0 "${BRIDGE_PID}" 2>/dev/null; then + echo "Error: bridge exited during startup. Last lines of ${LOGFILE}:" + tail -n 20 "${LOGFILE}" + rm -f "${PIDFILE}" + exit 1 +fi +echo "✓ MQTT bridge started" diff --git a/tools/start_mqtt_broker.sh b/tools/start_mqtt_broker.sh index 2d63223c..b4c3b34d 100644 --- a/tools/start_mqtt_broker.sh +++ b/tools/start_mqtt_broker.sh @@ -4,11 +4,13 @@ SOURCE_DIRECTORY="$(realpath "${SCRIPT_DIRECTORY}/..")" source "${SOURCE_DIRECTORY}/adore.env" 2>/dev/null || true -BROKER_HOST="${MQTT_BROKER_HOST:-localhost}" -BROKER_PORT="${MQTT_BROKER_PORT:-1883}" +# MQTT_BROKER_HOST/PORT are the pre-rename names, still honoured. +BROKER_HOST="${MQTT_HOST:-${MQTT_BROKER_HOST:-127.0.0.1}}" +BROKER_PORT="${MQTT_PORT:-${MQTT_BROKER_PORT:-1883}}" LOG_DIR="${SOURCE_DIRECTORY}/.log/mqtt" PIDFILE="${LOG_DIR}/mqtt_broker.pid" LOGFILE="${LOG_DIR}/mqtt_broker.log" +CONFFILE="${LOG_DIR}/mqtt_broker.conf" mkdir -p "${LOG_DIR}" @@ -26,21 +28,32 @@ if ! command -v mosquitto &>/dev/null; then exit 1 fi -echo "Starting mosquitto -> ${LOGFILE}" -mosquitto -p "${BROKER_PORT}" -d >> "${LOGFILE}" 2>&1 -sleep 1 +# mosquitto 2.x refuses every connection without an explicit listener and +# anonymous grant, so the settings are written out rather than passed as flags. +cat > "${CONFFILE}" < ${LOGFILE}" +mosquitto -c "${CONFFILE}" >> "${LOGFILE}" 2>&1 & +BROKER_PID=$! +# Own PID rather than pgrep, which would latch onto an unrelated instance. echo "${BROKER_PID}" > "${PIDFILE}" echo " pid ${BROKER_PID}" +for _ in $(seq 25); do + nc -z -w1 "${BROKER_HOST}" "${BROKER_PORT}" 2>/dev/null && break + kill -0 "${BROKER_PID}" 2>/dev/null || break + sleep 0.2 +done + if ! nc -z -w3 "${BROKER_HOST}" "${BROKER_PORT}" 2>/dev/null; then echo "Error: broker not reachable at ${BROKER_HOST}:${BROKER_PORT}. Check ${LOGFILE}" + tail -n 20 "${LOGFILE}" + rm -f "${PIDFILE}" exit 1 fi echo "✓ MQTT broker reachable at ${BROKER_HOST}:${BROKER_PORT}" From fbe719a54d0a95e3d9fa9af0d1fc5a73c9d6438c Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Tue, 11 Aug 2026 14:27:11 +0200 Subject: [PATCH 15/18] updated grpc message bridge node Signed-off-by: Andrew Koerner --- .../config/bridge_config.yaml | 7 + .../grpc_message_bridge/bridge_node.py | 45 +++ .../tools/test_integration.py | 363 ++++++++++++++++-- 3 files changed, 382 insertions(+), 33 deletions(-) mode change 100755 => 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_integration.py diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml index d42d5c33..c4bbb770 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/config/bridge_config.yaml @@ -3,3 +3,10 @@ enabled_streams: client_streams: - stream: client.ClientService.ClientStream + # The gateway streams nothing until it receives a SubscriptionRequest. + # Replayed on every reconnect. An empty payload leaves the oneof unset. + initial_send: + - field: subscription + payload: + type: ALL + vehicle_ids: [] diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py index 1b5bb1e4..e7287ca2 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py @@ -1,3 +1,4 @@ +import json import logging import os import queue @@ -145,6 +146,7 @@ def _setup_client_streams(self): self._setup_send_subscriptions(stream) send_queue = self.stream_send_queues.setdefault(stream.key, queue.Queue()) + self._seed_initial_send(stream, entry) def _sender(sq=send_queue, shutdown=self.shutdown_event, s=stream): """Replay the last message per oneof field, then stream live ones until shutdown.""" @@ -190,6 +192,49 @@ def _worker(addr=remote_addr, s=stream, sq=send_queue, sender=_sender): threading.Thread(target=_worker, daemon=True).start() self.get_logger().info(f'Client stream: {key} -> {remote_addr}') + # ------------------------------------------------------------------ + # Initial send-direction messages + # ------------------------------------------------------------------ + + def _seed_initial_send(self, stream: StreamDef, entry: dict): + """ + Prime _last_sent so the stream opens with these messages already queued. + + Send fields are normally driven by ROS publishers, which means a stream + whose remote peer waits for a request (a subscription, a handshake) stays + silent until something publishes. _sender replays _last_sent at the start + of every connection, so seeding here also covers reconnects. + """ + for item in entry.get('initial_send', []): + field = item.get('field') + fm = stream.send_field_map.get(field) + if fm is None: + self.get_logger().error( + f'[{stream.key}] initial_send: unknown send field {field!r}') + continue + + payload = item.get('payload', {}) + if not payload: + self.get_logger().error( + f'[{stream.key}] initial_send {field}: empty payload leaves the ' + f'oneof unset, the peer will not see this message') + continue + + try: + msg = proto_field_set( + stream.send_msg_cls, field, json.dumps(payload).encode(), fm.format) + except Exception as e: + self.get_logger().error(f'[{stream.key}] initial_send {field}: {e}') + continue + + if active_oneof_field(msg) != field: + self.get_logger().error( + f'[{stream.key}] initial_send {field}: oneof did not take') + continue + + self._last_sent.setdefault(stream.key, {})[field] = msg + self.get_logger().info(f'[{stream.key}] initial send: {field} {payload}') + # ------------------------------------------------------------------ # Server streams (identical to client but called from _setup_grpc_server) # ------------------------------------------------------------------ diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_integration.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_integration.py old mode 100755 new mode 100644 index 3ecf5a35..dd2a940f --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_integration.py +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/test_integration.py @@ -2,14 +2,16 @@ """ Integration tests -- Supervision External Gateway gTA Integration Guide Phase 1. -Covers all 7 test scenarios from section 8 of the spec. +Covers the 7 test scenarios from section 8 of the spec, plus scenario 8 which +checks the ROS side of the bridge. Usage: set -a && source .secrets.env && set +a - PYTHONPATH=proto/generated python3 scripts/test_integration.py [--scenario N] + PYTHONPATH=proto/generated python3 tools/test_integration.py [--scenario N] """ import argparse +import collections import json import os import sys @@ -20,8 +22,11 @@ import urllib.request sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'proto', 'generated')) +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) import grpc +from google.protobuf import text_format +from google.protobuf.json_format import MessageToDict from client import stream_pb2, stream_pb2_grpc from messages import common_pb2 @@ -32,23 +37,188 @@ _GREEN = '\033[92m' _RED = '\033[91m' _YELLOW = '\033[93m' +_CYAN = '\033[96m' _RESET = '\033[0m' _results = [] +MAX_BODY = 400 +DUMP_LIMIT = 3 + + def _pass(scenario, msg): print(f' {_GREEN}PASS{_RESET} {msg}') - _results.append((scenario, True, msg)) + _results.append((scenario, 'pass', msg)) def _fail(scenario, msg): print(f' {_RED}FAIL{_RESET} {msg}') - _results.append((scenario, False, msg)) + _results.append((scenario, 'fail', msg)) + +def _warn(scenario, msg): + print(f' {_YELLOW}WARN{_RESET} {msg}') + _results.append((scenario, 'warn', msg)) def _info(msg): print(f' {msg}') -def _fetch_token(client_id, client_secret, expect_success=True): +# --------------------------------------------------------------------------- +# Topic map -- single source of truth is the bridge's own proto_registry +# --------------------------------------------------------------------------- + +STREAM_KEY = 'client.ClientService.ClientStream' + +Route = collections.namedtuple('Route', 'topic ros_msg_type format') + +_FALLBACK_SEND = { + 'subscription': Route('/supervision/subscription', 'std_msgs/msg/String', 'json'), + 'ack': Route('/supervision/ack/tx', 'std_msgs/msg/String', 'json'), + 'signal_message': Route('/supervision/signaling/tx', 'std_msgs/msg/String', 'json'), +} +_FALLBACK_RECV = { + 'telemetry': Route('/supervision/telemetry', 'std_msgs/msg/String', 'json'), + 'notification': Route('/supervision/notifications', 'std_msgs/msg/String', 'json'), + 'ack': Route('/supervision/ack/rx', 'std_msgs/msg/String', 'json'), + 'signal_message': Route('/supervision/signaling/rx', 'std_msgs/msg/String', 'json'), +} + + +def _load_topic_map(): + try: + from grpc_message_bridge.proto_registry import STREAMS + s = STREAMS[STREAM_KEY] + as_routes = lambda fms: { + fm.field_name: Route(fm.ros_topic, fm.ros_msg_type, fm.format) for fm in fms} + return as_routes(s.send_fields), as_routes(s.recv_fields), 'proto_registry' + except Exception as e: + return _FALLBACK_SEND, _FALLBACK_RECV, f'fallback ({type(e).__name__}: {e})' + + +SEND_ROUTES, RECV_ROUTES, _TOPIC_SOURCE = _load_topic_map() + +SEND_TOPICS = {f: r.topic for f, r in SEND_ROUTES.items()} +RECV_TOPICS = {f: r.topic for f, r in RECV_ROUTES.items()} + + +def _print_topic_map(): + print(f'\nTopic map (source: {_TOPIC_SOURCE})') + print(f' {"direction":<14}{"proto message":<16}{"oneof field":<16}' + f'{"ROS topic":<30}{"ROS type":<22}format') + for f, r in SEND_ROUTES.items(): + print(f' {"ros -> grpc":<14}{"ClientMessage":<16}{f:<16}' + f'{r.topic:<30}{r.ros_msg_type:<22}{r.format}') + for f, r in RECV_ROUTES.items(): + print(f' {"grpc -> ros":<14}{"ServerMessage":<16}{f:<16}' + f'{r.topic:<30}{r.ros_msg_type:<22}{r.format}') + + missing = [f.name for f in stream_pb2.ServerMessage.DESCRIPTOR.oneofs[0].fields + if f.name not in RECV_ROUTES] + if missing: + print(f' {_YELLOW}ServerMessage oneof branches with no ROS route: ' + f'{missing}{_RESET}') + + +def _trunc(s): + return s if len(s) <= MAX_BODY else s[:MAX_BODY] + f'... (+{len(s) - MAX_BODY} chars)' + + +def _to_dict(msg, defaults=False): + kwargs = {'preserving_proto_field_name': True} + if defaults: + try: + return MessageToDict(msg, always_print_fields_with_no_presence=True, **kwargs) + except TypeError: + return MessageToDict(msg, including_default_value_fields=True, **kwargs) + return MessageToDict(msg, **kwargs) + + +def _payload_json(msg, field): + """Exactly what the bridge puts in std_msgs/String.data for this field.""" + return json.dumps(_to_dict(getattr(msg, field))) + + +def _envelope(msg): + """The non-oneof metadata the bridge never forwards to ROS.""" + if not msg.HasField('metadata'): + return '' + return json.dumps(_to_dict(msg.metadata)) + + +def _dump_grpc(received): + """ + Print every message received on the stream, its oneof branch, the ROS topic + that branch routes to, and the payload the bridge would publish there. + Messages with no oneof set are shown too, since the bridge drops them silently. + """ + if not received: + _info(f'{_YELLOW}no gRPC messages received on the stream{_RESET}') + return {} + + counts = collections.Counter(m.WhichOneof('payload') for m in received) + summary = ', '.join(f'{f or ""}={n}' for f, n in counts.most_common()) + _info(f'{len(received)} message(s): {summary}') + + shown = collections.Counter() + for i, m in enumerate(received): + field = m.WhichOneof('payload') + key = field or '' + if shown[key] >= DUMP_LIMIT: + continue + shown[key] += 1 + + route = RECV_ROUTES.get(field) + print() + _info(f'{_CYAN}[msg {i}]{_RESET} ServerMessage.{key}') + _info(f' metadata {_trunc(_envelope(m))}') + + if field is None: + _info(f' ros topic {_YELLOW}none, oneof unset so the bridge drops ' + f'this message{_RESET}') + _dump_body(m, 'ServerMessage') + continue + + if route is None: + _info(f' ros topic {_YELLOW}none, {field!r} has no recv_fields entry in ' + f'proto_registry so the bridge drops it{_RESET}') + else: + _info(f' ros topic {_GREEN}{route.topic}{_RESET} ' + f'[{route.ros_msg_type}, format={route.format}]') + + payload = getattr(m, field) + _info(f' ros data {_trunc(_payload_json(m, field))}') + _dump_body(payload, type(payload).DESCRIPTOR.full_name) + + for key, total in counts.items(): + if shown[key or ''] < total: + _info(f' ... {total - shown[key or ""]} more ' + f'{key or ""} message(s) not shown') + + unroutable = sum(n for f, n in counts.items() if f not in RECV_ROUTES) + if unroutable: + _info(f'{_YELLOW}{unroutable} of {len(received)} message(s) have no ROS route ' + f'and never reach a topic{_RESET}') + return dict(counts) + + +def _dump_body(msg, type_name): + """ + proto3 omits defaults, so an all-default message serialises to '{}' and looks + like missing data. Print the wire size and an explicit defaults-included view + so an empty payload can be told apart from an unread one. + """ + wire = len(msg.SerializeToString()) + raw = text_format.MessageToString(msg, as_one_line=True).strip() + + _info(f' proto type {type_name} ({wire} wire byte{"" if wire == 1 else "s"})') + if wire == 0: + _info(f' {_YELLOW}fields set none, every field is at its proto3 default, so the ' + f'gateway sent an empty message{_RESET}') + else: + _info(f' fields set {_trunc(raw)}') + _info(f' with defaults {_trunc(json.dumps(_to_dict(msg, defaults=True)))}') + + +def _fetch_token(client_id, client_secret): params = urllib.parse.urlencode({ 'grant_type': 'client_credentials', 'client_id': client_id, @@ -79,7 +249,7 @@ def _make_channel(token): def _open_stream(channel, messages_fn, timeout=5): - stub = stream_pb2_grpc.ClientServiceStub(channel) + stub = stream_pb2_grpc.ClientServiceStub(channel) received = [] error = [None] call = [None] @@ -169,7 +339,7 @@ def scenario_2(): _info(f'HTTP {status}: {data}') if status == 401: - _pass(2, f'HTTP 401 Unauthorized as expected') + _pass(2, 'HTTP 401 Unauthorized as expected') else: _fail(2, f'Expected HTTP 401, got {status}') @@ -201,6 +371,8 @@ def scenario_3(valid_token): def scenario_4(valid_token): print('\nScenario 4: Telemetry subscription (happy path)') + _info(f'Subscribing TELEMETRY, target ROS topic {RECV_TOPICS.get("telemetry", "")}') + channel = _make_channel(valid_token) received, error = _open_stream(channel, _subscription_msg(stream_pb2.TELEMETRY), timeout=8) channel.close() @@ -209,18 +381,22 @@ def scenario_4(valid_token): _fail(4, f'{error.code().name}: {error.details()!r}') return + counts = _dump_grpc(received) telemetry_msgs = [m for m in received if m.WhichOneof('payload') == 'telemetry'] - _info(f'Received {len(received)} messages total, {len(telemetry_msgs)} telemetry') if telemetry_msgs: t = telemetry_msgs[0].telemetry _info(f'vehicle_id={t.vehicle_id} connected={t.is_connected} ' f'state={t.telemetry.state} velocity={t.telemetry.velocity:.2f}') - _pass(4, f'Stream opened, received {len(telemetry_msgs)} VehicleTelemetryUpdate message(s)') - elif not error: - _pass(4, 'Stream opened successfully (no vehicles active -- no telemetry received)') + _pass(4, f'Received {len(telemetry_msgs)} VehicleTelemetryUpdate message(s) ' + f'for {RECV_TOPICS["telemetry"]}') + elif received: + other = ', '.join(f or '' for f in counts) + _warn(4, f'Subscribed TELEMETRY and got {len(received)} message(s) but no ' + f'telemetry: {other}. Nothing reaches {RECV_TOPICS["telemetry"]}.') else: - _fail(4, 'Stream timed out with no messages') + _warn(4, 'Stream opened but the gateway sent nothing. Nothing can reach ROS ' + 'from an empty stream, so a green run here only proves auth works.') # --------------------------------------------------------------------------- @@ -252,6 +428,8 @@ def scenario_5(valid_token): _fail(5, f'{error.code().name}: {error.details()!r}') return + _dump_grpc(filtered) + wrong_vehicle = [ m for m in filtered if m.WhichOneof('payload') == 'telemetry' @@ -261,6 +439,8 @@ def scenario_5(valid_token): if wrong_vehicle: _fail(5, f'Received telemetry for unrequested vehicles: ' f'{[m.telemetry.vehicle_id for m in wrong_vehicle]}') + elif not filtered: + _warn(5, f'Filter untested, zero messages returned for vehicle_ids={vehicle_ids}') else: _pass(5, f'All {len(filtered)} messages matched requested vehicle_ids={vehicle_ids}') @@ -298,6 +478,8 @@ def _gen(): received2, error2 = _open_stream(channel2, _subscription_msg(stream_pb2.ALL), timeout=5) channel2.close() + _dump_grpc(received2) + if error2 and error2.code() not in (grpc.StatusCode.DEADLINE_EXCEEDED,): _fail(6, f'Reconnect failed: {error2.code().name}: {error2.details()!r}') else: @@ -335,17 +517,105 @@ def _bad_request(): # --------------------------------------------------------------------------- -# Entry point +# Scenario 8: ROS side of the bridge # --------------------------------------------------------------------------- +def scenario_8(duration=15, trigger=True): + print('\nScenario 8: ROS topic data (requires a running bridge)') + + try: + import rclpy + from rclpy.node import Node + from rclpy.qos import (DurabilityPolicy, HistoryPolicy, QoSProfile, + ReliabilityPolicy) + from std_msgs.msg import String + except ImportError as e: + _warn(8, f'rclpy unavailable ({e}), ROS side not checked') + return + + sub_topic = SEND_TOPICS.get('subscription') + + rclpy.init() + node = Node('bridge_integration_probe') + qos = QoSProfile( + depth = 10, + history = HistoryPolicy.KEEP_LAST, + reliability = ReliabilityPolicy.RELIABLE, + durability = DurabilityPolicy.VOLATILE, + ) + + received = {topic: [] for topic in RECV_TOPICS.values()} + for topic in received: + node.create_subscription( + String, topic, + lambda msg, t=topic: received[t].append(msg.data), + qos) + _info(f'Subscribed to: {", ".join(sorted(received))}') + + pub = node.create_publisher(String, sub_topic, qos) if trigger else None + _spin(node, 2.0) + _print_ros_graph(node, sub_topic) + + if pub is not None: + request = json.dumps({'type': 'ALL', 'vehicleIds': []}) + _info(f'{_CYAN}ROS{_RESET} publish -> {sub_topic} {request}') + for _ in range(3): + pub.publish(String(data=request)) + _spin(node, 0.5) + + _info(f'Listening on ROS for {duration}s') + _spin(node, duration) + + node.destroy_node() + rclpy.shutdown() + + print() + total = 0 + for topic in sorted(received): + msgs = received[topic] + total += len(msgs) + colour = _GREEN if msgs else _YELLOW + _info(f'{colour}{len(msgs):>5}{_RESET} {topic}') + for data in msgs[:DUMP_LIMIT]: + _info(f' {_trunc(data)}') + + if total: + live = sum(1 for v in received.values() if v) + _pass(8, f'{total} ROS message(s) across {live} topic(s)') + else: + _fail(8, 'No data on any ROS topic. The gRPC scenarios above can pass while ' + 'the bridge publishes nothing.') + + +def _spin(node, seconds): + import rclpy + deadline = time.time() + seconds + while time.time() < deadline: + rclpy.spin_once(node, timeout_sec=0.1) + + +def _print_ros_graph(node, sub_topic): + live = dict(node.get_topic_names_and_types()) + _info('ROS graph:') + for topic in sorted(set(RECV_TOPICS.values()) | {sub_topic}): + types = live.get(topic) + pubs = len(node.get_publishers_info_by_topic(topic)) + subs = len(node.get_subscriptions_info_by_topic(topic)) + colour = _GREEN if types else _YELLOW + _info(f' {colour}{topic:<34}{_RESET}' + f'{types[0] if types else "":<24}pubs={pubs} subs={subs}') + + # The bridge advertises every recv topic at startup, so absence means it is not up. + if not any(t in live for t in RECV_TOPICS.values()): + _info(f'{_YELLOW}Bridge publishers not visible. Check `make start`, ROS_DOMAIN_ID ' + f'and that the container shares this network namespace.{_RESET}') + # --------------------------------------------------------------------------- # Inspect mode -- subscribe and pretty-print all incoming messages # --------------------------------------------------------------------------- def inspect(duration=30, vehicle_ids=None, sub_type=stream_pb2.ALL): - import base64 - status, data = _fetch_token(os.environ['CLIENT_ID'], os.environ['CLIENT_SECRET']) if status != 200 or 'access_token' not in data: print(f'Token fetch failed: {data}') @@ -385,12 +655,14 @@ def _gen(): continue counts[field] = counts.get(field, 0) + 1 - ts = time.strftime('%H:%M:%S') + ts = time.strftime('%H:%M:%S') + topic = RECV_TOPICS.get(field, '') if field == 'telemetry': - t = msg.telemetry + t = msg.telemetry tel = t.telemetry - print(f'[{ts}] TELEMETRY vehicle={t.vehicle_id} connected={t.is_connected}') + print(f'[{ts}] TELEMETRY -> {topic} vehicle={t.vehicle_id} ' + f'connected={t.is_connected}') print(f' state={tel.state} ' f'pos=({tel.position.lat:.5f}, {tel.position.lon:.5f}) ' f'heading={tel.heading:.3f}rad ' @@ -401,21 +673,23 @@ def _gen(): elif field == 'notification': n = msg.notification.notification - print(f'[{ts}] NOTIFICATION id={msg.notification.id} ' + print(f'[{ts}] NOTIFICATION -> {topic} id={msg.notification.id} ' f'vehicle={msg.notification.vehicle_id}') - print(f' [{n.severity}] {n.title}: {n.message} ' - f'node={n.ros_node}') + print(f' [{n.severity}] {n.title}: {n.message} node={n.ros_node}') elif field == 'ack': - print(f'[{ts}] ACK ok={msg.ack.success} msg={msg.ack.error_msg!r}') + print(f'[{ts}] ACK -> {topic} ok={msg.ack.success} msg={msg.ack.error_msg!r}') elif field == 'signal_message': - s = msg.signal_message + s = msg.signal_message inner = s.WhichOneof('payload') - print(f'[{ts}] SIGNAL session={s.session_id} peer={s.peer_id} type={inner}') + print(f'[{ts}] SIGNAL -> {topic} session={s.session_id} ' + f'peer={s.peer_id} type={inner}') else: - print(f'[{ts}] {field.upper()} {getattr(msg, field)}') + print(f'[{ts}] {field.upper()} -> {topic} {getattr(msg, field)}') + + print(f' ros_payload: {_trunc(_payload_json(msg, field))}') except KeyboardInterrupt: if call[0]: @@ -428,10 +702,14 @@ def _gen(): print(f'\nReceived: {counts if counts else "nothing"}') +# --------------------------------------------------------------------------- +# Entry point +# --------------------------------------------------------------------------- + if __name__ == '__main__': parser = argparse.ArgumentParser() parser.add_argument('--scenario', type=int, default=0, - help='Run a single scenario (1-7). Default: run all.') + help='Run a single scenario (1-8). Default: run all.') parser.add_argument('--inspect', action='store_true', help='Subscribe and print all incoming messages.') parser.add_argument('--duration', type=int, default=30, @@ -441,17 +719,31 @@ def _gen(): parser.add_argument('--type', choices=['ALL','TELEMETRY','NOTIFICATIONS'], default='ALL', dest='sub_type', help='Subscription type for inspect (default: ALL).') + parser.add_argument('--ros-duration', type=int, default=15, + help='Scenario 8 ROS listen window in seconds (default: 15).') + parser.add_argument('--no-ros-trigger', action='store_true', + help='Scenario 8: listen only, do not publish a subscription request.') + parser.add_argument('--full', action='store_true', + help='Print payloads untruncated.') + parser.add_argument('--dump', type=int, default=DUMP_LIMIT, + help=f'Sample payloads printed per field (default: {DUMP_LIMIT}).') args = parser.parse_args() + if args.full: + MAX_BODY = 10 ** 9 + DUMP_LIMIT = args.dump + for var in ('AUTH_ENDPOINT', 'GRPC_ENDPOINT', 'CLIENT_ID', 'CLIENT_SECRET'): if not os.environ.get(var): print(f'Missing env var: {var}') print('Run: set -a && source .secrets.env && set +a') sys.exit(1) - print(f'Auth: {os.environ["AUTH_ENDPOINT"]}') - print(f'gRPC: {os.environ["GRPC_ENDPOINT"]}') - print(f'Client: {os.environ["CLIENT_ID"]}') + print(f'Auth: {os.environ["AUTH_ENDPOINT"]}') + print(f'gRPC: {os.environ["GRPC_ENDPOINT"]}') + print(f'Client: {os.environ["CLIENT_ID"]}') + print(f'ROS domain: {os.environ.get("ROS_DOMAIN_ID", "0 (default)")}') + _print_topic_map() if args.inspect: sub_type_map = {'ALL': stream_pb2.ALL, 'TELEMETRY': stream_pb2.TELEMETRY, @@ -477,13 +769,18 @@ def _gen(): run(5, scenario_5, token) run(6, scenario_6, token) run(7, scenario_7, token) + run(8, scenario_8, args.ros_duration, not args.no_ros_trigger) if args.scenario == 0: - passed = sum(1 for _, ok, _ in _results if ok) - failed = sum(1 for _, ok, _ in _results if not ok) + passed = sum(1 for _, s, _ in _results if s == 'pass') + failed = sum(1 for _, s, _ in _results if s == 'fail') + warned = sum(1 for _, s, _ in _results if s == 'warn') print(f'\n{"="*50}') print(f'Results: {_GREEN}{passed} passed{_RESET} {_RED}{failed} failed{_RESET} ' - f'of {len(_results)} checks') + f'{_YELLOW}{warned} warned{_RESET} of {len(_results)} checks') + for n, status, msg in _results: + if status != 'pass': + colour = _RED if status == 'fail' else _YELLOW + print(f' {colour}{status.upper():<5}{_RESET} scenario {n}: {msg}') if failed: sys.exit(1) - From 868e2ad014a10fd6c49061a91663247bb99bdaaf Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Tue, 11 Aug 2026 14:46:33 +0200 Subject: [PATCH 16/18] fixed race condition in grpc bridge Signed-off-by: Andrew Koerner --- .../grpc_message_bridge/bridge_node.py | 82 ++++++++++++-- .../tools/ros_echo_json.py | 101 ++++++++++++++++++ 2 files changed, 175 insertions(+), 8 deletions(-) create mode 100644 ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/ros_echo_json.py diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py index e7287ca2..34f235c5 100644 --- a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/grpc_message_bridge/bridge_node.py @@ -1,3 +1,4 @@ +import collections import json import logging import os @@ -69,6 +70,11 @@ def __init__(self): self._last_sent: dict = {} # stream.key -> {oneof_field: proto_msg} self.shutdown_event = threading.Event() + self._recv_counts = collections.Counter() # oneof field -> messages off the wire + self._pub_counts = collections.Counter() # ros topic -> messages published + self._drop_counts = collections.Counter() # reason -> messages discarded + self._last_stats = None + self._grpc_server = None self._grpc_channels: dict = {} @@ -80,6 +86,7 @@ def __init__(self): self._setup_server_streams() self._setup_client_streams() self.create_timer(0.01, self._drain_publish_queue) + self.create_timer(float(self.config.get('stats_interval', 5.0)), self._log_stats) # ------------------------------------------------------------------ # Publisher setup @@ -163,31 +170,63 @@ def _sender(sq=send_queue, shutdown=self.shutdown_event, s=stream): continue def _worker(addr=remote_addr, s=stream, sq=send_queue, sender=_sender): + attempt = 0 while not self.shutdown_event.is_set(): + attempt += 1 + gen = None + opened = time.monotonic() + count = 0 try: - channel = self._channel(addr) + # A stream torn down by the peer can leave the cached channel + # unusable, so every retry after the first gets a fresh one. + channel = self._channel(addr, reset=attempt > 1) stub = s.stub_cls(channel) rpc = getattr(stub, s.rpc) + seeded = list(self._last_sent.get(s.key, {})) + self.get_logger().info( + f'[{s.key}] opening stream (attempt {attempt}), ' + f'replaying {seeded or "nothing"}') if s.stream_type == 'bidi': - for recv_msg in rpc(sender()): + gen = sender() + for recv_msg in rpc(gen): + count += 1 + if count == 1: + self.get_logger().info( + f'[{s.key}] first message after ' + f'{time.monotonic() - opened:.1f}s') self._dispatch_recv(s, recv_msg) elif s.stream_type == 'server_streaming': req = s.send_msg_cls() for recv_msg in rpc(req): + count += 1 self._dispatch_recv(s, recv_msg) elif s.stream_type == 'client_streaming': - rpc(sender()) + gen = sender() + rpc(gen) + + self.get_logger().warn( + f'[{s.key}] stream ended cleanly after ' + f'{time.monotonic() - opened:.1f}s and {count} message(s), ' + f'reconnecting in 2s') + time.sleep(2) except grpc.RpcError as e: self.get_logger().warn( - f'[{s.key}] {e.code().name}: {e.details()} -- reconnecting in 2s') + f'[{s.key}] {e.code().name} after ' + f'{time.monotonic() - opened:.1f}s and {count} message(s): ' + f'{e.details()} -- reconnecting in 2s') time.sleep(2) except Exception: self.get_logger().error(f'[{s.key}] worker: {traceback.format_exc()}') time.sleep(2) + finally: + # Without this the old generator stays parked on sq.get and + # competes with the next connection's sender for the queue. + if gen is not None: + gen.close() threading.Thread(target=_worker, daemon=True).start() self.get_logger().info(f'Client stream: {key} -> {remote_addr}') @@ -278,12 +317,16 @@ def cb(ros_msg, f=fm, cls=stream.send_msg_cls, sq=send_queue, ser=serialize): def _dispatch_recv(self, stream: StreamDef, proto_msg): field_name = active_oneof_field(proto_msg) - fm = stream.recv_field_map.get(field_name) + self._recv_counts[field_name or ''] += 1 + + fm = stream.recv_field_map.get(field_name) if fm is None: + self._drop_counts[f'no recv_fields entry: {field_name or ""}'] += 1 return payload = proto_field_to_bytes(proto_msg, field_name, fm.format) if payload is None: + self._drop_counts[f'encode returned None: {field_name}'] += 1 return wire_type = wire_ros_type(fm.ros_msg_type, fm.format) @@ -293,16 +336,38 @@ def _dispatch_recv(self, stream: StreamDef, proto_msg): try: ros_msg = deserialize(payload) except Exception as e: + self._drop_counts[f'deserialize failed: {field_name}'] += 1 self.get_logger().error(f'[{stream.key}] deser {field_name}: {e}') return pub = self.ros_pubs.get(fm.ros_topic) - if pub: - self.ros_publish_queue.put((pub, ros_msg)) + if pub is None: + self._drop_counts[f'no publisher: {fm.ros_topic}'] += 1 + self.get_logger().warn(f'[{stream.key}] no publisher for {fm.ros_topic}') + return + + self.ros_publish_queue.put((pub, ros_msg)) + + def _log_stats(self): + state = (dict(self._recv_counts), dict(self._pub_counts), dict(self._drop_counts)) + if state == self._last_stats: + return + self._last_stats = state + + fmt = lambda c: ', '.join(f'{k}={v}' for k, v in sorted(c.items())) or 'none' + self.get_logger().info(f'grpc recv: {fmt(self._recv_counts)}') + self.get_logger().info(f'ros pub: {fmt(self._pub_counts)}') + if self._drop_counts: + self.get_logger().warn(f'dropped: {fmt(self._drop_counts)}') # ------------------------------------------------------------------ - def _channel(self, address: str) -> grpc.Channel: + def _channel(self, address: str, reset: bool = False) -> grpc.Channel: + if reset and address in self._grpc_channels: + try: + self._grpc_channels.pop(address).close() + except Exception: + pass if address not in self._grpc_channels: self._grpc_channels[address] = make_channel(address) return self._grpc_channels[address] @@ -311,6 +376,7 @@ def _drain_publish_queue(self): while not self.ros_publish_queue.empty(): pub, msg = self.ros_publish_queue.get_nowait() pub.publish(msg) + self._pub_counts[pub.topic_name] += 1 def shutdown(self): self.shutdown_event.set() diff --git a/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/ros_echo_json.py b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/ros_echo_json.py new file mode 100644 index 00000000..95a2cc18 --- /dev/null +++ b/ros2_workspace/src/adore_interfaces/grpc_message_bridge/tools/ros_echo_json.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +""" +Echo the bridge's ROS topics with each String payload parsed as JSON. + + ros2 run --prefix 'python3' ... # not needed, just run it directly + python3 tools/ros_echo_json.py [--topic T ...] [--compact] [--field F ...] +""" + +import argparse +import json +import os +import sys +import time + +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..', 'proto', 'generated')) +sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..')) + +import rclpy +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from std_msgs.msg import String + +_DEFAULT_TOPICS = [ + '/supervision/telemetry', + '/supervision/notifications', + '/supervision/ack/rx', + '/supervision/signaling/rx', +] + + +def _topics(): + try: + from grpc_message_bridge.proto_registry import STREAMS + stream = STREAMS['client.ClientService.ClientStream'] + return [fm.ros_topic for fm in stream.recv_fields] + except Exception: + return _DEFAULT_TOPICS + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument('--topic', action='append', dest='topics', + help='Topic to echo, repeatable. Default: all bridge recv topics.') + ap.add_argument('--field', action='append', dest='fields', + help='Print only these top-level JSON keys, repeatable.') + ap.add_argument('--compact', action='store_true', help='One line per message.') + args = ap.parse_args() + + topics = args.topics or _topics() + + rclpy.init() + node = Node('ros_echo_json') + qos = QoSProfile( + depth = 10, + history = HistoryPolicy.KEEP_LAST, + reliability = ReliabilityPolicy.RELIABLE, + durability = DurabilityPolicy.VOLATILE, + ) + + width = max(len(t) for t in topics) + seen = {t: 0 for t in topics} + + def on_msg(msg, topic): + seen[topic] += 1 + stamp = time.strftime('%H:%M:%S') + try: + payload = json.loads(msg.data) + except ValueError: + print(f'[{stamp}] {topic:<{width}} #{seen[topic]} {msg.data}', + flush=True) + return + + if args.fields and isinstance(payload, dict): + payload = {k: payload[k] for k in args.fields if k in payload} + + body = (json.dumps(payload) if args.compact + else json.dumps(payload, indent=2, sort_keys=True)) + print(f'[{stamp}] {topic:<{width}} #{seen[topic]}', flush=True) + print(body if args.compact is False else f' {body}', flush=True) + + for topic in topics: + node.create_subscription(String, topic, + lambda m, t=topic: on_msg(m, t), qos) + + print(f'Echoing {len(topics)} topic(s) as JSON, Ctrl-C to stop:', file=sys.stderr) + for topic in topics: + print(f' {topic}', file=sys.stderr) + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + print('\nmessages per topic: ' + + ', '.join(f'{t}={n}' for t, n in seen.items()), file=sys.stderr) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() From 767faae76171fa2fdafd8db85f540c6d76e79270 Mon Sep 17 00:00:00 2001 From: Andrew Koerner Date: Fri, 14 Aug 2026 02:56:13 +0200 Subject: [PATCH 17/18] updated mqtt readme Signed-off-by: Andrew Koerner --- .../adore_interfaces/mqtt_message_bridge/README.md | 13 +++---------- 1 file changed, 3 insertions(+), 10 deletions(-) diff --git a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/README.md b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/README.md index f43d98b3..73c1bd55 100644 --- a/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/README.md +++ b/ros2_workspace/src/adore_interfaces/mqtt_message_bridge/README.md @@ -29,16 +29,9 @@ Each mapping supports optional QoS overrides: `qos_depth`, `qos_durability` (`vo ## DiMOS 5.2 interface `config/bridge_config.yaml` targets the UseCase 5.2 broker -(`mqtts://broker-imoger.dev.dimos-ops.com:8887`, MQTT v3.1.1, QoS 1) using the -certificate file names the specification hands out: `imoger-rootCA.crt`, -`dlr-client.crt`, `dlr-client.key`. Credentials come from `.mqtt_secrets.env`. - -Subscribed topics: - -| MQTT | ROS 2 | -| --- | --- | -| `od_imoger/solbox/+/notifications` | `/imoger/solbox/notifications` | -| `od_imoger/vehicles/dlr1/nmea` | `/imoger/vehicles/dlr1/nmea` | +(`mqtts://broker:8887`, MQTT v3.1.1, QoS 1) using the +certificate file names the specification hands out: `ca.crt`, +`client.crt`, `client.key`. Credentials come from `.mqtt_secrets.env`. Payloads are UTF-8 JSON and are bridged verbatim (`format: raw`) into `std_msgs/msg/String`. `scripts/dimos_message.py` builds a notification message From 5582da9387c8512b3fe926877f003060e48909bb Mon Sep 17 00:00:00 2001 From: MikkelDLR Date: Tue, 25 Aug 2026 12:18:30 +0200 Subject: [PATCH 18/18] updated env to not automatically start mqtt bridge Signed-off-by: MikkelDLR --- adore.env | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/adore.env b/adore.env index 98710983..b3f494e7 100644 --- a/adore.env +++ b/adore.env @@ -87,7 +87,7 @@ ZENOH_BRIDGE_ROUTER=tcp/localhost:7447 # - Set to "true" to launch the bridge node alongside the CLI container. # - Set to "false" or leave unset to disable. # - Default: false -MQTT_BRIDGE_ENABLE=true +MQTT_BRIDGE_ENABLE=false # MQTT_BROKER_HOST: Hostname or IP of the MQTT broker the bridge connects to. # - Default: localhost