diff --git a/README.md b/README.md index c62fcc8..2a4a4a4 100644 --- a/README.md +++ b/README.md @@ -1,521 +1,176 @@ # Autonomy Metrics Logger -`autonomy_metrics_logger` is a ROS 2 node that: - -- Tracks travelled distance, autonomous distance, incidents, and collisions. -- Logs interventions and collisions to MongoDB with a full snapshot of system state. -- Is configured entirely via a YAML file (topics, fields, triggers, dynamic publishers). -- Supports both explicit operation mode (`/gophar/operation_mode`) and inferred mode (odom + velocity). - ---- - -## 1. High-Level Behaviour - -The node: - -1. Loads a YAML config describing topics and logging behaviour. -2. Subscribes to all configured topics. -3. Maintains a live `system_snapshot` dict with the latest values from each topic. -4. Updates metrics: - - `distance` (total odometry distance) - - `autonomous_distance` (distance when mode = Autonomous) - - `incidents` (interventions in autonomous mode, plus manual overrides) - - `collision_incidents` (collision monitor only, separate from incidents) -5. Logs events to MongoDB **only** when something happens: - - Manual override (explicit or observed) - - E-stop - - Faults / joy overrides / other YAML triggers - - Collisions (nav vs collision velocity) -6. Each event stored in Mongo includes: - - `metrics` (distance, autonomous_distance, speed, battery) - - `system_snapshot` (current values of all configured topics) - - Event type & extra details. - -MDBI is computed as: - -> `mdbi = autonomous_distance / incidents` -> (If `incidents == 0`, `mdbi = autonomous_distance`.) - ---- - -## 2. MongoDB Integration - -The node creates a document in `robot_incidents.sessions` per run: - -```json -{ - "session_start_time": "...", - "robot_name": "...", - "farm_name": "...", - "field_name": "...", - "application": "...", - "scenario_name": "...", - "aoc_repos_info": [ - { - "aoc_scenario_path": { - "path": "...", - "exists": true, - "remote": "...", - "branch": "...", - "commit": "...", - "short_commit": "...", - "commit_message": "...", - "tags": ["v0.1.0"], - "describe": "v0.1.0-3-gXXXXXXX", - "dirty": false, - "error": null - } - } - ], - "mdbi": 123.4, - "incidents": 5, - "distance": 456.7, - "autonomous_distance": 321.0, - "collision_incidents": 2, - "events": [ - { - "time": "...", - "event_type": "Manual_override", - "details": { - "operation_mode": "Manual", - "estop": false, - "metrics": { - "distance": 12.3, - "autonomous_distance": 12.3, - "speed": 0.4, - "battery_percentage": 78.0 - }, - "system_snapshot": { - "/gophar/system_status": { - "fault_shutdown": false, - "hv_on": true, - "battery_percentage": 78.0, - "...": "..." - }, - "/gps_base/fix": { "...": "..." }, - "odometry": { "x": 1.23, "y": 4.56, "vx": 0.4 }, - "...": "..." - } - } - } - ] -} -```` - -Environment variables used to tag the session: - -* `ROBOT_NAME` -* `FARM_NAME` -* `FIELD_NAME` -* `APPLICATION` -* `SCENARIO_NAME` - ---- - -## 3. Node Parameters - -All parameters are exposed through the launch file. - -### Core configuration - -| Parameter | Type | Default | Description | -| ------------------------ | ------ | ------------- | ------------------------------------------------------ | -| `config_yaml` | string | `""` | Path to YAML config file for topics & triggers. | -| `mongodb_host` | string | `"localhost"` | Local MongoDB host. | -| `mongodb_port` | int | `27017` | Local MongoDB port. | -| `remote_mongodb_host` | string | `""` | Remote MongoDB host (empty = disabled). | -| `remote_mongodb_port` | int | `27017` | Remote MongoDB port. | -| `enable_remote_logging` | bool | `false` | Enable writes to remote MongoDB. | -| `min_distance_threshold` | double | `0.2` | Min odom step (m) required for distance update. | -| `stop_timeout` | double | `2.0` | Time (s) after last odom update before speed is set 0. | - -### Mode observer (fallback mode inference) - -Used **only** if no topic with `role: "control_mode"` is configured. - -| Parameter | Type | Default | Description | -| ------------------------------- | ------ | ------- | ------------------------------------------------------------------------ | -| `mode_observer_cmd_timeout` | double | `1.0` | Commands on `mode_observer` topic are “recent” within this many seconds. | -| `mode_observer_speed_threshold` | double | `0.01` | Robot is considered moving if speed > this (m/s). | - -Logic when active: - -* Topic with `role: "mode_observer"` (e.g. `/cmd_vel/collision_smoothed`) marks last command time. -* If odometry reports movement and: - - * cmd is recent → mode = Autonomous - * cmd is not recent → mode = Manual -* When observed mode changes from Auto → Manual: - - * `Manual_override_observed` event is logged. - * An incident is counted (MDBI). - -### Collision monitor (nav vs collision velocity) - -| Parameter | Type | Default | Description | -| -------------------------- | ------ | ------- | --------------------------------------------------------------------------------------------- | -| `collision_nav_threshold` | double | `0.01` | `/cmd_vel/nav.linear.x` must be > this to count as a forward command. | -| `collision_zero_threshold` | double | `0.001` | `/cmd_vel/collision.linear.x` considered zero if abs(x) ≤ this. | -| `collision_time_window` | double | `0.5` | Nav and collision commands must both be within this time window (s). | -| `collision_log_cooldown` | double | `1.0` | Optional legacy cooldown between collisions (can be unused if using pure falling-edge logic). | - -Collision detection (falling-edge): - -* Keep last `/cmd_vel/nav` and `/cmd_vel/collision`. -* Compute a collision **only when**: - - * Previous `/cmd_vel/collision.linear.x` had velocity (`> collision_zero_threshold`), - * Current `/cmd_vel/collision.linear.x` is ~0, - * Latest `/cmd_vel/nav.linear.x` > `collision_nav_threshold`, - * Commands are recent (`≤ collision_time_window`). -* On collision: - - * `collision_incidents` is incremented, - * A `Collision` event is logged with snapshot, - * Total collisions published on `mdbi_logger/total_collision_incidents` (Int32). - ---- - -## 4. YAML Config File - -### Top-level structure +This repository now contains two ROS 2 Python packages under `src/`: + +- `autonomy_metrics`: the rewritten metrics/logger node. +- `autonomy_metrics_webui`: a web dashboard backend + frontend for live metrics and MongoDB session history. + +## What The Rewrite Tracks + +The logger is now driven by the YAML config and implements the requested behavior directly: + +- Autonomous/manual mode comes from `/autonomous_mode` (`std_msgs/msg/Bool`). +- An intervention is counted only when `/autonomous_mode` switches `true -> false` and at least one of these heartbeat topics is `false`: + - `/autonomy_checks/heartbeat` + - `/navigation_safe/heartbeat` + - `/system_check/heartbeat` +- MDBI is computed from autonomous distance and counted interventions: + - `mdbi = autonomous_distance / interventions` + - if interventions are still `0`, MDBI is reported as undefined until the first intervention +- `/robot_state` (`std_msgs/msg/String`) is monitored for `Active -> Disabled` transitions while in autonomous mode. +- `/gophar_vehicle_controller/odometry` is used for: + - total travelled distance + - manual/autonomous travelled distance + - current linear speed + - average speed +- `/robot_navigation_area` is used for: + - current area + - travelled distance by area + - total time by area + - average speed by area + - average time per visit by area +- `/plan` plus `/odometry/global` are used for: + - current path deviation + - average path deviation + - maximum path deviation +- Git repository metadata is still captured at session start using the same configured repo list. +- A node heartbeat is published on `/autonomy_metrics/heartbeat`. +- A database snapshot is written every `n` seconds, default `10`, for topics marked `snapshot: true` in the YAML config. +- Writes still go to the local MongoDB and optionally to the remote/cloud MongoDB. + +## Config File + +The system behavior is declared in `src/autonomy_metrics/config/metrics_full.yaml`. + +Top-level structure: ```yaml -git_repos: - aoc_scenario_path: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" - -topics: - - name: "/gps_base/fix" - type: "sensor_msgs/msg/NavSatFix" - log_fields: - - "latitude" - - "longitude" - - "altitude" - publish: - enable: true - topic: "mdbi_logger/gps_alt" - type: "std_msgs/msg/Float32" - field: "altitude" - - - name: "/gophar_vehicle_controller/odometry" - type: "nav_msgs/msg/Odometry" - role: "odometry" - log_fields: - - "pose.pose.position.x" - - "pose.pose.position.y" - - "twist.twist.linear.x" - publish: - enable: false - - - name: "/gophar/system_status" - type: "dynium_gophar_interfaces/msg/SystemStatus" - role: "system_status" - log_all_fields: false - log_fields: - - "fault_shutdown" - - "hv_on" - - "ignition_switch" - - "charger_interlock" - - "battery_percentage" - battery_field: "battery_percentage" - publish: - enable: true - topic: "mdbi_logger/battery_level" - type: "std_msgs/msg/Float32" - field: "battery_percentage" - intervention_on_change: - fault_shutdown: - trigger_value: true - event_type: "Fault_shutdown" - - - name: "/gophar/operation_mode" - type: "std_msgs/msg/String" - role: "control_mode" - mode_field: "data" - mode_mapping: - "AUTO": "Autonomous" - "MANUAL": "Manual" - "1": "Autonomous" - "0": "Manual" - - - name: "/cmd_vel/joy" - type: "geometry_msgs/msg/Twist" - log_fields: [] - intervention_on_message: - enable: true - event_type: "Joy_override" - - - name: "/cmd_vel/collision_smoothed" - type: "geometry_msgs/msg/Twist" - role: "mode_observer" - log_fields: [] - - - name: "/cmd_vel/nav" - type: "geometry_msgs/msg/Twist" - role: "collision_nav" - log_fields: [] - - - name: "/cmd_vel/collision" - type: "geometry_msgs/msg/Twist" - role: "collision_output" - log_fields: [] - - - name: "/gophar/steering_actuator/front" - type: "dynium_gophar_interfaces/msg/SteeringActuatorStatus" - log_all_fields: true - log_fields: [] - - - name: "/gophar/steering_actuator/rear" - type: "dynium_gophar_interfaces/msg/SteeringActuatorStatus" - log_all_fields: true - log_fields: [] - - - name: "/gophar/motor_status/motor_0" - type: "dynium_gophar_interfaces/msg/MotorStatus" - log_all_fields: true - log_fields: [] - - - name: "/gophar/motor_status/motor_1" - type: "dynium_gophar_interfaces/msg/MotorStatus" - log_all_fields: true - log_fields: [] -``` - -### Per-topic keys - -Common fields: - -* `name` - ROS topic name to subscribe to. - -* `type` - ROS message type string. Accepts: - - * `pkg/msg/MessageName` (e.g. `"sensor_msgs/msg/NavSatFix"`), or - * `pkg/MessageName` (e.g. `"sensor_msgs/NavSatFix"`). - -* `role` (optional, lowercase in logic) - Special roles: - - * `"odometry"`: used for distance/speed computation. - * `"control_mode"`: explicit operation mode (`Autonomous` vs `Manual`). - * `"estop"`: emergency stop; used to generate `EMS` incidents. - * `"system_status"`: can carry `battery_field` and fault triggers. - * `"mode_observer"`: used by mode observer (e.g. `/cmd_vel/collision_smoothed`). - * `"collision_nav"`: nav velocity command. - * `"collision_output"`: collision-limited velocity command. - -* `log_fields` (list of dotted paths) - Fields to extract into `system_snapshot[topic_name]`. - Only used if `log_all_fields` is `false`. - -* `log_all_fields` (bool, default: false) - If `true`, the full message is converted to a dict and stored in `system_snapshot[topic_name]`. - Ignores `log_fields`. +git_repos_base_path: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" -* `battery_field` (optional, string) - Field name under this topic that carries battery percentage; updates `current_battery`. +git_repos: + scenario: "." + autonomy_metrics_logger: "jabas/autonomy_metrics_logger" -#### Dynamic republishing +runtime: + heartbeat_topic: "/autonomy_metrics/heartbeat" + session_summary_topic: "/autonomy_metrics/session_summary_json" + heartbeat_period_sec: 1.0 + snapshot_interval_sec: 10.0 + distance_epsilon_m: 0.01 + stale_speed_timeout_sec: 2.0 -```yaml -publish: - enable: true - topic: "mdbi_logger/battery_level" - type: "std_msgs/msg/Float32" - field: "battery_percentage" +topics: + - name: "/autonomous_mode" + type: "std_msgs/msg/Bool" + role: "autonomous_mode" + snapshot: true + + - name: "/autonomy_checks/heartbeat" + type: "std_msgs/msg/Bool" + role: "intervention_heartbeat" + metric_key: "autonomy_checks" + snapshot: true ``` -* `enable`: `true`/`false` -* `topic`: output topic name -* `type`: output message type -* `field`: dotted field in input message; mapped to `.data` of the output (with type casting). - -#### Control mode - -For topics with `role: "control_mode"`: - -* `mode_field` (string) - Dotted field path inside message holding the raw mode value (e.g. `"data"`). - -* `mode_mapping` (dict) - Map raw values to `"Autonomous"` or `"Manual"` - Example: `"AUTO" → "Autonomous"`, `"0" → "Manual"`. - If unmapped and `str(value) == "3"`, forced to Manual; otherwise treated as Autonomous. - -Changing from `Autonomous` to `Manual`: - -* Ends an autonomy segment (time tracking). -* Logs `Manual_override` event. -* Increments `incidents` (MDBI) even if now in Manual (forced by `force_count=True`). - -#### E-stop role - -For `role: "estop"` topics, additional config: - -* `field` (optional, default `"data"`) - Boolean field to check. - When it changes and becomes `True`: +### Supported Roles - * `EMS` intervention is logged. - * Incidents increment only if mode = Autonomous. +- `autonomous_mode`: exactly one topic, must be a boolean mode source. +- `intervention_heartbeat`: one or more boolean health gates used when mode changes from autonomous to manual. +- `robot_state`: exactly one string topic with values such as `Disabled`, `Enabled`, `Active`. +- `distance_odometry`: exactly one odometry topic for travelled distance and speed. +- `navigation_area`: exactly one string topic for `INSIDE_POLYTUNNEL`, `OUTSIDE_POLYTUNNEL`, or `TRANSITION_INTO_POLYTUNNEL`. +- `global_path`: exactly one `nav_msgs/msg/Path` topic. +- `global_pose_odometry`: exactly one odometry topic used for path deviation against the path. -#### YAML-driven interventions +### Snapshot Logging -Per-topic: +Set `snapshot: true` on any configured topic you want included in the periodic MongoDB snapshot. The snapshot stores: -* `intervention_on_message`: +- the current computed summary +- the latest value seen for each configured snapshot topic +- a timestamp - ```yaml - intervention_on_message: - enable: true - event_type: "Joy_override" - ``` +## MongoDB Layout - If enabled, **every message** on that topic triggers an event of this type via `trigger_intervention`. +The rewritten logger stores data in: -* `intervention_on_change`: +- `sessions`: one document per run with session metadata, Git information, and the latest summary +- `session_events`: event documents such as interventions, mode transitions, robot-state transitions, and area transitions +- `session_snapshots`: periodic point-in-time snapshots of metrics and selected topics - ```yaml - intervention_on_change: - fault_shutdown: - trigger_value: true - event_type: "Fault_shutdown" - ``` +## Published ROS Topics - * Key (`fault_shutdown`) is a field name in `data`. - * `trigger_value` optional; if present, event fires only when field changes **and** equals `trigger_value`. - * `event_type` is the event name stored in DB. +- `/autonomy_metrics/heartbeat` +- `/autonomy_metrics/current_mode` +- `/autonomy_metrics/interventions` +- `/autonomy_metrics/mdbi` +- `/autonomy_metrics/active_to_disabled_in_autonomous` +- `/autonomy_metrics/total_distance_m` +- `/autonomy_metrics/autonomous_distance_m` +- `/autonomy_metrics/manual_distance_m` +- `/autonomy_metrics/current_speed_mps` +- `/autonomy_metrics/average_speed_mps` +- `/autonomy_metrics/autonomous_time_sec` +- `/autonomy_metrics/manual_time_sec` +- `/autonomy_metrics/current_navigation_area` +- `/autonomy_metrics/path_deviation/current_m` +- `/autonomy_metrics/path_deviation/average_m` +- `/autonomy_metrics/path_deviation/max_m` +- `/autonomy_metrics/session_summary_json` ---- +## Launching -## 5. Mode Handling and Incidents +Build the two packages: -* Default mode at startup is set to `Autonomous`: - - ```python - self.AUTO = 'Autonomous' - self.MAN = 'Manual' - self.details = {'estop': False, 'operation_mode': self.AUTO} - ``` - -* `incidents` are only incremented when: - - * `trigger_intervention(...)` is called and: - - * `operation_mode == "Autonomous"`, or - * `force_count=True` (for explicit/observed manual overrides). - -* E-stop, faults, joy overrides, etc. in **Manual** mode: - - * Are logged to DB (event + snapshot), - * Do **not** increment `incidents`, thus do not affect MDBI. - ---- - -## 6. Launching the Node - -Example launch file (simplified; adapt package/executable names): - -```python -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node - -def generate_launch_description(): - config_yaml_arg = DeclareLaunchArgument( - 'config_yaml', - default_value='', - description='Path to AutonomyMetricsLogger YAML config', - ) - - # ... declare other args (mongodb_host, ports, thresholds, etc.) ... - - config_yaml = LaunchConfiguration('config_yaml') - - metrics_logger_node = Node( - package='autonomy_metrics', # adjust - executable='metric_logger', # adjust - name='mdbi_logger_dynamic', - output='screen', - parameters=[{ - 'config_yaml': config_yaml, - 'mongodb_host': LaunchConfiguration('mongodb_host'), - 'mongodb_port': LaunchConfiguration('mongodb_port'), - 'remote_mongodb_host': LaunchConfiguration('remote_mongodb_host'), - 'remote_mongodb_port': LaunchConfiguration('remote_mongodb_port'), - 'enable_remote_logging': LaunchConfiguration('enable_remote_logging'), - 'min_distance_threshold': LaunchConfiguration('min_distance_threshold'), - 'stop_timeout': LaunchConfiguration('stop_timeout'), - 'mode_observer_cmd_timeout': LaunchConfiguration('mode_observer_cmd_timeout'), - 'mode_observer_speed_threshold': LaunchConfiguration('mode_observer_speed_threshold'), - 'collision_nav_threshold': LaunchConfiguration('collision_nav_threshold'), - 'collision_zero_threshold': LaunchConfiguration('collision_zero_threshold'), - 'collision_time_window': LaunchConfiguration('collision_time_window'), - 'collision_log_cooldown': LaunchConfiguration('collision_log_cooldown'), - }], - ) - - return LaunchDescription([ - config_yaml_arg, - # ... other DeclareLaunchArgument ... - metrics_logger_node, - ]) +```bash +colcon build --packages-select autonomy_metrics autonomy_metrics_webui ``` -### Example launch usage - -Minimal: +Launch the logger and the dashboard together: ```bash -ros2 launch autonomy_metrics_logger autonomy_metrics_logger.launch.py \ - config_yaml:=/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/autonomy_metrics_logger/config/metrics_gophar.yaml +ros2 launch autonomy_metrics autonomy_metrics.launch.py \ + config_yaml:=/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/autonomy_metrics_logger/src/autonomy_metrics/config/metrics_full.yaml \ + mongodb_host:=localhost \ + mongodb_port:=27018 \ + enable_remote_logging:=true \ + remote_mongodb_host:=YOUR_CLOUD_MONGO_HOST \ + remote_mongodb_port:=27017 \ + use_webui:=true \ + ui_host:=0.0.0.0 \ + ui_port:=8081 ``` -With remote DB + tuned thresholds: +If you only want the dashboard package: ```bash -ros2 launch autonomy_metrics_logger autonomy_metrics_logger.launch.py \ - config_yaml:=/path/to/metrics_gophar.yaml \ +ros2 launch autonomy_metrics_webui autonomy_metrics_webui.launch.py \ mongodb_host:=localhost \ - mongodb_port:=27017 \ - enable_remote_logging:=true \ - remote_mongodb_host:=10.0.0.42 \ - remote_mongodb_port:=27017 \ - min_distance_threshold:=0.1 \ - stop_timeout:=1.5 \ - mode_observer_cmd_timeout:=0.8 \ - mode_observer_speed_threshold:=0.02 \ - collision_nav_threshold:=0.02 \ - collision_zero_threshold:=0.001 \ - collision_time_window:=0.4 + mongodb_port:=27018 \ + ui_port:=8081 ``` ---- +## Web UI + +The web UI serves: -## 7. Testing Tips +- live summary from `/autonomy_metrics/session_summary_json` +- heartbeat freshness from `/autonomy_metrics/heartbeat` +- recent events and snapshots from MongoDB -* Verify topics: +Default URL: - ```bash - ros2 topic list - ros2 topic info /gophar_vehicle_controller/odometry - ros2 topic info /gophar/system_status - ``` +```text +http://0.0.0.0:8081 +``` -* Trigger joystick interventions: +Open it from a browser on the host machine using the machine IP or loopback address. - ```bash - ros2 topic pub --once /cmd_vel/joy geometry_msgs/msg/Twist \ - "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" - ``` +## Notes On MDBI -* Check DB content (Mongo shell or Mongo client) in `robot_incidents.sessions`. +The previous package also used the standard autonomous-distance-per-intervention formula, but its intervention counting relied on several unrelated triggers. The rewrite keeps the core metric definition and narrows the intervention rule to your requested standard: -The README covers: +- only count autonomous to manual transitions +- only count them when at least one configured heartbeat gate is false -* All node parameters, -* YAML configuration options, -* How incidents, MDBI, collisions, and snapshots are computed, -* How to launch and test the system. +That makes MDBI directly reflect the operational quality of autonomous runs instead of mixing in joystick or collision heuristics. diff --git a/src/autonomy_metrics/autonomy_metrics/db_mgr.py b/src/autonomy_metrics/autonomy_metrics/db_mgr.py index 9595bc9..0e99ad6 100644 --- a/src/autonomy_metrics/autonomy_metrics/db_mgr.py +++ b/src/autonomy_metrics/autonomy_metrics/db_mgr.py @@ -1,117 +1,161 @@ -from pymongo import MongoClient +from __future__ import annotations + from datetime import datetime, timezone +import math + from bson import ObjectId +from pymongo import DESCENDING, MongoClient + -# NEW try: import numpy as np -except Exception: +except Exception: # pragma: no cover np = None class DatabaseMgr: - """ - A class to manage database operations for robot session. - """ - - def __init__(self, database_name='robot_incidents', host='localhost', port=27017): + """MongoDB helper for session summaries, events, and periodic snapshots.""" + + def __init__( + self, + database_name: str = "robot_incidents", + host: str = "localhost", + port: int = 27017, + ): self.client = MongoClient( - f'mongodb://{host}:{port}/', - connectTimeoutMS=None + f"mongodb://{host}:{port}/", + connectTimeoutMS=1500, + serverSelectionTimeoutMS=1500, ) self.db = self.client[database_name] - self.sessions_collection = self.db['sessions'] + self.sessions_collection = self.db["sessions"] + self.events_collection = self.db["session_events"] + self.snapshots_collection = self.db["session_snapshots"] self.session_id = None - # NEW + self.sessions_collection.create_index([("session_start_time", DESCENDING)]) + self.events_collection.create_index([("session_id", 1), ("time", DESCENDING)]) + self.snapshots_collection.create_index([("session_id", 1), ("time", DESCENDING)]) + def _bson_safe(self, obj): - """Recursively convert non-BSON types (notably numpy) into Mongo-friendly types.""" if np is not None: - # numpy arrays -> list if isinstance(obj, np.ndarray): return obj.tolist() - # numpy scalar -> python scalar if isinstance(obj, np.generic): return obj.item() + if isinstance(obj, float): + if math.isfinite(obj): + return obj + return None + if isinstance(obj, dict): - return {k: self._bson_safe(v) for k, v in obj.items()} + return {key: self._bson_safe(value) for key, value in obj.items()} if isinstance(obj, (list, tuple)): - return [self._bson_safe(v) for v in obj] + return [self._bson_safe(value) for value in obj] return obj - def init_session(self, env_variables, aoc_repos_info): + def _json_safe(self, obj): + if isinstance(obj, ObjectId): + return str(obj) + if isinstance(obj, datetime): + return obj.astimezone(timezone.utc).isoformat() + if isinstance(obj, dict): + return {key: self._json_safe(value) for key, value in obj.items()} + if isinstance(obj, list): + return [self._json_safe(value) for value in obj] + return obj + + def init_session(self, env_variables: dict, git_repos_info: list[dict], metadata: dict): session_start = datetime.now(tz=timezone.utc) - session_document = { - "session_start_time": session_start, - "robot_name": env_variables['robot_name'], - "farm_name": env_variables['farm_name'], - "field_name": env_variables['field_name'], - "application": env_variables['application'], - "scenario_name": env_variables['scenario_name'], - "aoc_repos_info": aoc_repos_info, - "mdbi": None, - "incidents": 0, - "distance": 0, - "autonomous_distance": 0, - "collision_incidents": 0, - "events": [] - } - - # NEW: sanitize (in case aoc_repos_info contains numpy types) - session_document = self._bson_safe(session_document) + session_document = self._bson_safe( + { + "session_start_time": session_start, + "session_end_time": None, + "robot_name": env_variables.get("robot_name", "UNDEFINED"), + "farm_name": env_variables.get("farm_name", "UNDEFINED"), + "field_name": env_variables.get("field_name", "UNDEFINED"), + "application": env_variables.get("application", "UNDEFINED"), + "scenario_name": env_variables.get("scenario_name", "UNDEFINED"), + "aoc_repos_info": git_repos_info, + "summary": metadata, + } + ) result = self.sessions_collection.insert_one(session_document) self.session_id = result.inserted_id + return str(self.session_id) - def add_event(self, event): - # NEW: sanitize event before writing - event = self._bson_safe(event) - + def update_session_summary(self, summary: dict): + if self.session_id is None: + return False + summary = self._bson_safe(summary) result = self.sessions_collection.update_one( - {"_id": ObjectId(self.session_id)}, - {"$addToSet": {"events": event}} + {"_id": self.session_id}, + {"$set": {"summary": summary, "last_updated_time": datetime.now(tz=timezone.utc)}}, ) - return result.modified_count > 0 - - def update_incidents(self, incidents): - incidents = self._bson_safe(incidents) # harmless; keeps pattern consistent + return result.acknowledged + + def mark_session_end(self, summary: dict | None = None): + if self.session_id is None: + return False + update = {"session_end_time": datetime.now(tz=timezone.utc)} + if summary is not None: + update["summary"] = self._bson_safe(summary) result = self.sessions_collection.update_one( - {"_id": ObjectId(self.session_id)}, - {"$set": {"incidents": incidents}} + {"_id": self.session_id}, + {"$set": update}, ) - return result.modified_count > 0 - - def update_distance(self, distance): - distance = self._bson_safe(distance) - result = self.sessions_collection.update_one( - {"_id": ObjectId(self.session_id)}, - {"$set": {"distance": distance}} + return result.acknowledged + + def add_event(self, event: dict): + if self.session_id is None: + return False + document = self._bson_safe( + { + "session_id": self.session_id, + **event, + } ) - return result.modified_count > 0 - - def update_autonomous_distance(self, autonomous_distance): - autonomous_distance = self._bson_safe(autonomous_distance) - result = self.sessions_collection.update_one( - {"_id": ObjectId(self.session_id)}, - {"$set": {"autonomous_distance": autonomous_distance}} + result = self.events_collection.insert_one(document) + return bool(result.inserted_id) + + def add_snapshot(self, snapshot: dict): + if self.session_id is None: + return False + document = self._bson_safe( + { + "session_id": self.session_id, + **snapshot, + } ) - return result.modified_count > 0 + result = self.snapshots_collection.insert_one(document) + return bool(result.inserted_id) - def update_mdbi(self, mdbi): - mdbi = self._bson_safe(mdbi) - result = self.sessions_collection.update_one( - {"_id": ObjectId(self.session_id)}, - {"$set": {"mdbi": mdbi}} + def fetch_latest_session(self) -> dict | None: + document = self.sessions_collection.find_one( + sort=[("session_start_time", DESCENDING)] ) - return result.modified_count > 0 - - def update_collision_incidents(self, collision_incidents): - collision_incidents = self._bson_safe(collision_incidents) - result = self.sessions_collection.update_one( - {"_id": ObjectId(self.session_id)}, - {"$set": {"collision_incidents": collision_incidents}} + return self._json_safe(document) if document else None + + def fetch_recent_events(self, limit: int = 20, session_id: str | None = None) -> list[dict]: + query = {} + if session_id: + query["session_id"] = ObjectId(session_id) + documents = list( + self.events_collection.find(query).sort("time", DESCENDING).limit(limit) + ) + return self._json_safe(documents) + + def fetch_recent_snapshots( + self, limit: int = 10, session_id: str | None = None + ) -> list[dict]: + query = {} + if session_id: + query["session_id"] = ObjectId(session_id) + documents = list( + self.snapshots_collection.find(query).sort("time", DESCENDING).limit(limit) ) - return result.modified_count > 0 + return self._json_safe(documents) diff --git a/src/autonomy_metrics/autonomy_metrics/git_utils.py b/src/autonomy_metrics/autonomy_metrics/git_utils.py new file mode 100644 index 0000000..b09c023 --- /dev/null +++ b/src/autonomy_metrics/autonomy_metrics/git_utils.py @@ -0,0 +1,89 @@ +"""Helpers for collecting git metadata for the configured repositories.""" + +from __future__ import annotations + +import os +import subprocess + + +DEFAULT_GIT_REPOS_ROOT = ( + "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" +) + + +def _run_git(repo_path: str, *args: str) -> str: + result = subprocess.run( + ["git", "-C", repo_path, *args], + capture_output=True, + text=True, + timeout=5.0, + check=False, + ) + if result.returncode != 0: + raise RuntimeError(result.stderr.strip() or "git command failed") + return result.stdout.strip() + + +def get_git_info(repo_path: str) -> dict: + """Return a git metadata snapshot for one repository path.""" + info = { + "path": repo_path, + "exists": False, + "remote": None, + "branch": None, + "commit": None, + "short_commit": None, + "commit_message": None, + "tags": [], + "describe": None, + "dirty": None, + "error": None, + } + + try: + if not repo_path or not os.path.isdir(repo_path): + info["error"] = "Path does not exist or is not a directory" + return info + + info["exists"] = True + + for key, command in ( + ("remote", ("config", "--get", "remote.origin.url")), + ("branch", ("rev-parse", "--abbrev-ref", "HEAD")), + ("commit", ("rev-parse", "HEAD")), + ("short_commit", ("rev-parse", "--short", "HEAD")), + ("commit_message", ("log", "-1", "--pretty=%s")), + ("describe", ("describe", "--tags", "--always")), + ): + try: + info[key] = _run_git(repo_path, *command) + except Exception: + info[key] = None + + try: + tags = _run_git(repo_path, "tag", "--points-at", "HEAD") + info["tags"] = tags.splitlines() if tags else [] + except Exception: + info["tags"] = [] + + try: + info["dirty"] = bool(_run_git(repo_path, "status", "--porcelain")) + except Exception: + info["dirty"] = None + + except Exception as exc: + info["error"] = str(exc) + + return info + + +def collect_git_repos_info(config: dict) -> list[dict]: + """Collect git metadata for all configured repositories.""" + repos_root = config.get("git_repos_base_path", DEFAULT_GIT_REPOS_ROOT) + repos_config = config.get("git_repos", {}) or {} + + results = [] + for label, rel_path in repos_config.items(): + repo_path = os.path.join(repos_root, rel_path) + results.append({label: get_git_info(repo_path)}) + return results diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index d6573a6..313e7d0 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -1,1087 +1,757 @@ #!/usr/bin/env python3 -""" -YAML-driven AutonomyMetricsLogger -Author: Ibrahim Hroob - JABASAI -""" +"""Config-driven ROS 2 autonomy metrics logger.""" -import os +from __future__ import annotations + +from collections import defaultdict +from datetime import datetime, timezone +import json import math -import subprocess +import os +from pathlib import Path + import yaml -import time -from datetime import datetime, timezone -from importlib import import_module import rclpy +from rclpy.executors import ExternalShutdownException from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data -from rclpy.time import Time - -# Keep a few explicit imports used by default publishers (std msgs) -from std_msgs.msg import Bool, Float32, Int32 - -from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr - - -def ros_msg_to_dict(msg): - """ - Recursively convert a ROS 2 Python message into a plain dict / list / primitive - so it can be safely stored in system_snapshot / Mongo. - - Works for nested messages and sequences. - """ - # Primitive types - if isinstance(msg, (bool, int, float, str)): - return msg - - # Sequences (lists, tuples, etc.) - if isinstance(msg, (list, tuple)): - return [ros_msg_to_dict(v) for v in msg] - # ROS 2 messages usually have get_fields_and_field_types() - if hasattr(msg, "get_fields_and_field_types"): - data = {} - for field_name in msg.get_fields_and_field_types().keys(): - try: - val = getattr(msg, field_name) - data[field_name] = ros_msg_to_dict(val) - except Exception: - # Best-effort; skip problematic fields - pass - return data - - # Fallback: try to convert __dict__ - if hasattr(msg, "__dict__"): - return { - k: ros_msg_to_dict(v) - for k, v in msg.__dict__.items() - if not k.startswith('_') - } - - # Last resort: return as-is - return msg - - -def import_msg_type(type_str: str): - """Dynamically imports message types based on string.""" - try: - parts = type_str.split('/') - if len(parts) == 2: - pkg, cls_name = parts - submodule = 'msg' - elif len(parts) == 3: - pkg, submodule, cls_name = parts - else: - raise ValueError("Message type must be 'pkg/msg/MessageName' or 'pkg/MessageName'") +from ament_index_python.packages import get_package_share_directory +from nav_msgs.msg import Odometry, Path as PathMsg +from std_msgs.msg import Bool, Float32, Int32, String - module = import_module(f"{pkg}.{submodule}") - return getattr(module, cls_name) - except Exception as e: - raise ImportError(f"Failed to import message type '{type_str}': {e}") +from autonomy_metrics.db_mgr import DatabaseMgr +from autonomy_metrics.git_utils import collect_git_repos_info +from autonomy_metrics.topic_utils import import_msg_type, ros_msg_to_dict -def get_nested_field(obj, path: str): - try: - cur = obj - for part in path.split('.'): - if '[' in part and part.endswith(']'): - name, idx = part[:-1].split('[') - cur = getattr(cur, name) - cur = cur[int(idx)] - else: - cur = getattr(cur, part) - return cur - except Exception as e: - raise AttributeError(f"Failed to get '{path}': {e}") +AUTONOMOUS_LABEL = "Autonomous" +MANUAL_LABEL = "Manual" +ROBOT_STATE_DISABLED = "Disabled" +ROBOT_STATE_ACTIVE = "Active" +KNOWN_AREAS = ( + "INSIDE_POLYTUNNEL", + "OUTSIDE_POLYTUNNEL", + "TRANSITION_INTO_POLYTUNNEL", +) class AutonomyMetricsLogger(Node): + """Main ROS 2 node for autonomy/session metrics and MongoDB logging.""" + def __init__(self): - super().__init__('mdbi_logger_dynamic') - - self.get_logger().info("Starting YAML-driven AutonomyMetricsLogger") - - # Parameters - self.declare_parameter('config_yaml', '/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/autonomy_metrics_logger/src/autonomy_metrics/config/metrics_full.yaml') - self.declare_parameter('mongodb_host', 'localhost') - self.declare_parameter('mongodb_port', 27018) - self.declare_parameter('remote_mongodb_host', '') - self.declare_parameter('remote_mongodb_port', 27017) - self.declare_parameter('enable_remote_logging', False) - self.declare_parameter('min_distance_threshold', 0.2) - self.declare_parameter('stop_timeout', 2.0) - self.declare_parameter('mode_observer_cmd_timeout', 1.0) # seconds - self.declare_parameter('mode_observer_speed_threshold', 0.01) # m/s - # Collision detection tuning - self.declare_parameter('collision_nav_threshold', 0.01) # nav cmd must be > this - self.declare_parameter('collision_zero_threshold', 0.001) # collision cmd abs(linear.x) <= this - self.declare_parameter('collision_time_window', 0.5) # seconds: nav & collision cmds must be close in time - self.declare_parameter('collision_log_cooldown', 1.0) # seconds between logged collisions - - self.collision_nav_threshold = self.get_parameter('collision_nav_threshold').get_parameter_value().double_value - self.collision_zero_threshold = self.get_parameter('collision_zero_threshold').get_parameter_value().double_value - self.collision_time_window = self.get_parameter('collision_time_window').get_parameter_value().double_value - self.collision_log_cooldown = self.get_parameter('collision_log_cooldown').get_parameter_value().double_value - self.mode_observer_cmd_timeout = self.get_parameter('mode_observer_cmd_timeout').get_parameter_value().double_value - self.mode_observer_speed_threshold = self.get_parameter('mode_observer_speed_threshold').get_parameter_value().double_value - self.config_path = self.get_parameter('config_yaml').get_parameter_value().string_value - self.mongo_host = self.get_parameter('mongodb_host').get_parameter_value().string_value - self.mongo_port = self.get_parameter('mongodb_port').get_parameter_value().integer_value - self.remote_mongo_host = self.get_parameter('remote_mongodb_host').get_parameter_value().string_value - self.remote_mongo_port = self.get_parameter('remote_mongodb_port').get_parameter_value().integer_value - self.enable_remote_logging = self.get_parameter('enable_remote_logging').get_parameter_value().bool_value - self.min_distance_threshold = self.get_parameter('min_distance_threshold').get_parameter_value().double_value - self.stop_timeout = self.get_parameter('stop_timeout').get_parameter_value().double_value - - self.get_logger().info(f"Config path: {self.config_path}") - self.get_logger().info( - f"Collision params: nav_thr={self.collision_nav_threshold}, " - f"zero_thr={self.collision_zero_threshold}, time_window={self.collision_time_window}" + super().__init__("autonomy_metrics_logger") + + default_config_path = os.path.join( + get_package_share_directory("autonomy_metrics"), + "config", + "metrics_full.yaml", ) - # DB Managers - self.db_mgr_local = DBMgr(host=self.mongo_host, port=self.mongo_port) - self.db_mgr_remote = None - if self.enable_remote_logging and self.remote_mongo_host: - try: - self.db_mgr_remote = DBMgr(host=self.remote_mongo_host, port=self.remote_mongo_port) - self.get_logger().info( - f"Remote DB logging enabled: {self.remote_mongo_host}:{self.remote_mongo_port}" - ) - except Exception as e: - self.get_logger().error(f"Failed to initialize remote DB manager: {e}") - - # Internal metrics state - self.mdbi = 0.0 - self.incidents = 0 - self.distance = 0.0 - self.autonomous_distance = 0.0 - self.autonomous_time = 0.0 - self.autonomous_start_time = None - self.AUTO = 'Autonomous' - self.MAN = 'Manual' - self.details = {'estop': False, 'operation_mode': self.AUTO} - - # Mode observation (velocity-based fallback when no "operation_mode" topic available) - self.has_explicit_control_mode = False - self.mode_observer_enabled = False - self.mode_observer_active = False - self.mode_observer_topic_name = None - self.last_auto_cmd_time = None - - # Collision monitoring - self.collision_incidents = 0 - self.collision_prev_has_velocity = False # for falling-edge detection - - self.last_nav_cmd = None - self.last_nav_time = None - self.last_collision_cmd = None - self.last_collision_time = None - self.last_collision_logged_time = None - - # ---------------------------------------------------- - # Global Snapshot Storage - # Stores the latest extracted values from all topics - # ---------------------------------------------------- - self.system_snapshot = {} - - # Snapshot of latest values per field for change detection - self.prev_field_values = {} - - # Current battery level (updated from any topic with 'battery_field') - self.current_battery = None - - # Odometry / Speed state - self.previous_x = None - self.previous_y = None - self.init_pose = True - self.speed = 0.0 - self.last_odom_update_time = self.get_clock().now() - - self.dynamic_subs = [] - self.dynamic_publishers = {} + self.declare_parameter("config_yaml", default_config_path) + self.declare_parameter("mongodb_host", "localhost") + self.declare_parameter("mongodb_port", 27018) + self.declare_parameter("remote_mongodb_host", "") + self.declare_parameter("remote_mongodb_port", 27017) + self.declare_parameter("enable_remote_logging", False) + self.declare_parameter("database_name", "robot_incidents") + + self.config_path = self.get_parameter("config_yaml").value + self.database_name = self.get_parameter("database_name").value + self.mongo_host = self.get_parameter("mongodb_host").value + self.mongo_port = self.get_parameter("mongodb_port").value + self.remote_mongo_host = self.get_parameter("remote_mongodb_host").value + self.remote_mongo_port = self.get_parameter("remote_mongodb_port").value + self.enable_remote_logging = self.get_parameter("enable_remote_logging").value + + self.config = self._load_config(self.config_path) + self.runtime_cfg = self.config.get("runtime", {}) + self.required_roles = defaultdict(list) self.topic_cfg_map = {} - # Static Publishers (Standard Metrics) - self.heartbeat_publisher = self.create_publisher(Bool, 'mdbi_logger/heartbeat', 10) - self.distance_publisher = self.create_publisher(Float32, 'mdbi_logger/total_traveled_distance', 10) - self.incidents_publisher = self.create_publisher(Int32, 'mdbi_logger/total_incidents', 10) - self.speed_publisher = self.create_publisher(Float32, 'mdbi_logger/robot_speed', 10) - self.collision_incidents_publisher = self.create_publisher(Int32, 'mdbi_logger/total_collision_incidents', 10) - - self.heartbeat_timer = self.create_timer(1.0, self.timer_callback) + self.heartbeat_period_sec = float( + self.runtime_cfg.get("heartbeat_period_sec", 1.0) + ) + self.snapshot_interval_sec = float( + self.runtime_cfg.get("snapshot_interval_sec", 10.0) + ) + self.distance_epsilon_m = float(self.runtime_cfg.get("distance_epsilon_m", 0.01)) + self.stale_speed_timeout_sec = float( + self.runtime_cfg.get("stale_speed_timeout_sec", 2.0) + ) + self.session_summary_topic = str( + self.runtime_cfg.get( + "session_summary_topic", "/autonomy_metrics/session_summary_json" + ) + ) + self.heartbeat_topic = str( + self.runtime_cfg.get("heartbeat_topic", "/autonomy_metrics/heartbeat") + ) - self.load_and_setup_config() + self.autonomous_mode = False + self.mode_entered_at = self._now_sec() + self.mode_totals_sec = { + AUTONOMOUS_LABEL: 0.0, + MANUAL_LABEL: 0.0, + } + self.intervention_count = 0 + self.intervention_heartbeat_states = {} + self.last_intervention_details = None - # Use the observer only if there is no explicit control_mode topic configured - self.mode_observer_active = self.mode_observer_enabled and not self.has_explicit_control_mode - if self.mode_observer_active: - self.get_logger().info( - f"Mode observer enabled using topic {self.mode_observer_topic_name} " - "(no explicit control_mode topic configured)" - ) + self.robot_state = ROBOT_STATE_DISABLED + self.active_to_disabled_in_autonomous_count = 0 - # Init Session - env_variables = { - 'robot_name': os.getenv('ROBOT_NAME', 'UNDEFINED'), - 'farm_name': os.getenv('FARM_NAME', 'UNDEFINED'), - 'field_name': os.getenv('FIELD_NAME', 'UNDEFINED'), - 'application': os.getenv('APPLICATION', 'UNDEFINED'), - 'scenario_name': os.getenv('SCENARIO_NAME', 'UNDEFINED'), + self.total_distance_m = 0.0 + self.distance_by_mode_m = { + AUTONOMOUS_LABEL: 0.0, + MANUAL_LABEL: 0.0, } + self.last_motion_position = None + self.first_motion_time_sec = None + self.last_motion_time_sec = None + self.last_speed_update_sec = None + self.current_speed_mps = 0.0 + + self.current_area = None + self.area_entered_at = None + self.area_totals_sec = defaultdict(float) + self.area_visit_count = defaultdict(int) + self.area_distance_m = defaultdict(float) + + self.current_path = [] + self.current_path_deviation_m = None + self.max_path_deviation_m = 0.0 + self.path_deviation_sum_m = 0.0 + self.path_deviation_samples = 0 + + self.latest_topic_values = {} + self.snapshot_topics = set() + self.topic_subscriptions = [] + + self._setup_publishers() + self._setup_databases() + self._setup_subscriptions() + self._init_session() + + self.heartbeat_timer = self.create_timer( + self.heartbeat_period_sec, self._heartbeat_timer_callback + ) + self.snapshot_timer = self.create_timer( + self.snapshot_interval_sec, self._snapshot_timer_callback + ) self.get_logger().info( - f"Session env: robot={env_variables['robot_name']}, " - f"farm={env_variables['farm_name']}, field={env_variables['field_name']}, " - f"app={env_variables['application']}, scenario={env_variables['scenario_name']}" + f"Autonomy metrics logger ready using config {self.config_path}" ) - git_repos = [] - if self.config: - try: - git_cfg = self.config.get('git_repos', {}) - for label, p in git_cfg.items(): - repo_path = os.path.join('/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario', p) - repo_info = self.get_git_info(repo_path) - git_repos.append({label: repo_info}) - self.get_logger().debug( - f"Git repo [{label}]: path={repo_info['path']}, " - f"branch={repo_info['branch']}, short_commit={repo_info['short_commit']}" - ) - except Exception as e: - self.get_logger().warn(f"Failed to collect git info: {e}") - - self.db_mgr_local.init_session(env_variables, git_repos) - if self.db_mgr_remote: - self.db_mgr_remote.init_session(env_variables, git_repos) - - def load_and_setup_config(self): - if not self.config_path: - self.config = {} - self.get_logger().warn("No config_yaml path provided; running with empty config.") - return - - try: - with open(self.config_path, 'r') as f: - self.config = yaml.safe_load(f) or {} - self.get_logger().info( - f"Loaded metrics config from YAML. topics={len(self.config.get('topics', []))}" - ) - except Exception as e: - self.get_logger().error(f"Failed to load YAML: {e}") - self.config = {} - return - - topics = self.config.get('topics', []) - - # Detect if an explicit control mode topic is configured - self.has_explicit_control_mode = any( - (t.get('role', '').lower() == 'control_mode') for t in topics + def _load_config(self, config_path: str) -> dict: + config_file = Path(config_path) + if not config_file.is_file(): + raise FileNotFoundError(f"Config file not found: {config_path}") + with config_file.open("r", encoding="utf-8") as handle: + return yaml.safe_load(handle) or {} + + def _setup_publishers(self): + self.heartbeat_publisher = self.create_publisher(Bool, self.heartbeat_topic, 10) + self.mode_publisher = self.create_publisher( + String, "/autonomy_metrics/current_mode", 10 + ) + self.interventions_publisher = self.create_publisher( + Int32, "/autonomy_metrics/interventions", 10 + ) + self.mdbi_publisher = self.create_publisher(Float32, "/autonomy_metrics/mdbi", 10) + self.robot_state_transition_publisher = self.create_publisher( + Int32, "/autonomy_metrics/active_to_disabled_in_autonomous", 10 + ) + self.total_distance_publisher = self.create_publisher( + Float32, "/autonomy_metrics/total_distance_m", 10 + ) + self.autonomous_distance_publisher = self.create_publisher( + Float32, "/autonomy_metrics/autonomous_distance_m", 10 + ) + self.manual_distance_publisher = self.create_publisher( + Float32, "/autonomy_metrics/manual_distance_m", 10 + ) + self.current_speed_publisher = self.create_publisher( + Float32, "/autonomy_metrics/current_speed_mps", 10 + ) + self.average_speed_publisher = self.create_publisher( + Float32, "/autonomy_metrics/average_speed_mps", 10 + ) + self.autonomous_time_publisher = self.create_publisher( + Float32, "/autonomy_metrics/autonomous_time_sec", 10 + ) + self.manual_time_publisher = self.create_publisher( + Float32, "/autonomy_metrics/manual_time_sec", 10 + ) + self.navigation_area_publisher = self.create_publisher( + String, "/autonomy_metrics/current_navigation_area", 10 + ) + self.path_deviation_current_publisher = self.create_publisher( + Float32, "/autonomy_metrics/path_deviation/current_m", 10 + ) + self.path_deviation_average_publisher = self.create_publisher( + Float32, "/autonomy_metrics/path_deviation/average_m", 10 + ) + self.path_deviation_max_publisher = self.create_publisher( + Float32, "/autonomy_metrics/path_deviation/max_m", 10 + ) + self.summary_publisher = self.create_publisher( + String, self.session_summary_topic, 10 ) - self.get_logger().info(f"Explicit control_mode topic present: {self.has_explicit_control_mode}") - - for item in topics: - name = item.get('name') - type_str = item.get('type') - - if not name or not type_str: - self.get_logger().warn(f"Skipping invalid topic config: {item}") - continue - - self.topic_cfg_map[name] = item - - role = item.get('role', '').lower() - - if role == 'mode_observer': - self.mode_observer_enabled = True - self.mode_observer_topic_name = name - self.get_logger().info(f"Mode observer configured on topic: {name}") - - # Dynamic Publisher Setup - pub_cfg = item.get('publish', {}) - if pub_cfg.get('enable', False): - try: - pub_msg_cls = import_msg_type(pub_cfg['type']) - pub = self.create_publisher(pub_msg_cls, pub_cfg['topic'], 10) - self.dynamic_publishers[name] = (pub, pub_msg_cls, pub_cfg.get('field')) - self.get_logger().info( - f"Dynamic publisher created: {pub_cfg['topic']} " - f"(type={pub_cfg['type']}) from topic={name}" - ) - except Exception as e: - self.get_logger().error(f"Pub creation failed for {name}: {e}") - self.dynamic_publishers[name] = None - else: - self.dynamic_publishers[name] = None - - # Subscription Setup - try: - msg_cls = import_msg_type(type_str) - role = item.get('role', '').lower() - - if role == 'odometry': - cb = lambda msg, n=name: self.odom_role_callback(n, msg) - elif role == 'control_mode': - cb = lambda msg, n=name: self.control_mode_role_callback(n, msg) - elif role == 'estop': - cb = lambda msg, n=name: self.estop_role_callback(n, msg) - elif role == 'collision_nav': - cb = lambda msg, n=name: self.collision_nav_callback(n, msg) - elif role == 'collision_output': - cb = lambda msg, n=name: self.collision_output_callback(n, msg) - else: - lf = item.get('log_fields', []) - cb = lambda msg, n=name, l=lf: self.generic_callback(n, msg, l) - - self.create_subscription(msg_cls, name, cb, qos_profile_sensor_data) - self.get_logger().info(f"Subscribed to {name} (role={role})") - - except Exception as e: - self.get_logger().error(f"Sub creation failed for {name}: {e}") - - def get_metrics_snapshot(self): - """ - Returns a lightweight metrics summary to attach to every event. - """ - metrics = { - "distance": self.distance, - "autonomous_distance": self.autonomous_distance, - "speed": self.speed, - } - if self.current_battery is not None: - metrics["battery_percentage"] = self.current_battery - return metrics - - def trigger_intervention(self, event_type: str, extra: dict | None = None, force_count: bool = False): - """ - Log an intervention-type event. - - - Only increments self.incidents when the robot is in Autonomous mode - (self.details['operation_mode'] == self.AUTO), unless force_count=True. - - Always logs the event (even in Manual) so history is preserved. - """ - if extra is None: - extra = {} - - current_mode = self.details.get('operation_mode', self.MAN) - count_incident = force_count or (current_mode == self.AUTO) - if count_incident: - self.incidents += 1 + def _setup_databases(self): + self.db_managers = [] - self.get_logger().info( - f"[Intervention] type={event_type}, mode={current_mode}, " - f"counted={count_incident}, total_incidents={self.incidents}" + local_db = DatabaseMgr( + database_name=self.database_name, + host=self.mongo_host, + port=self.mongo_port, ) + self.db_managers.append(local_db) - base_details = dict(self.details) # estop, operation_mode, etc. - base_details.update(extra) - base_details["system_snapshot"] = self.system_snapshot.copy() - - self.log_event(event_type, base_details) - - def handle_intervention_triggers(self, topic_name: str, data: dict, cfg: dict): - """ - Apply YAML-configured triggers for this topic. - - intervention_on_message: triggers on any message - - intervention_on_change[field]: triggers when selected field changes - """ - # 1) Trigger on any message (e.g. /cmd_vel/joy) - msg_trig = cfg.get("intervention_on_message", {}) - if msg_trig.get("enable", False): - evt_type = msg_trig.get("event_type", f"{topic_name}_activity") - self.get_logger().info( - f"[Trigger] intervention_on_message on topic '{topic_name}' -> event '{evt_type}'" + if self.enable_remote_logging and self.remote_mongo_host: + remote_db = DatabaseMgr( + database_name=self.database_name, + host=self.remote_mongo_host, + port=self.remote_mongo_port, ) - self.trigger_intervention(evt_type, extra={"topic": topic_name}) - - # 2) Trigger on change of specific fields - field_trigs = cfg.get("intervention_on_change", {}) - for field_name, trig_cfg in field_trigs.items(): - if field_name not in data: - continue - - key = (topic_name, field_name) - prev = self.prev_field_values.get(key) - new = data[field_name] - changed = (prev is None) or (prev != new) - self.prev_field_values[key] = new - - if not changed: - continue - - # Optional: only trigger on a specific value (e.g. True) - trigger_value = trig_cfg.get("trigger_value", None) - if trigger_value is not None and new != trigger_value: - continue - - evt_type = trig_cfg.get( - "event_type", f"{topic_name}:{field_name}_changed" + self.db_managers.append(remote_db) + + def _setup_subscriptions(self): + topics = self.config.get("topics", []) + if not topics: + raise RuntimeError("No topics configured in metrics YAML") + + for topic_cfg in topics: + topic_name = topic_cfg["name"] + topic_type = topic_cfg["type"] + role = topic_cfg.get("role", "").strip() + self.topic_cfg_map[topic_name] = topic_cfg + + if topic_cfg.get("snapshot", False): + self.snapshot_topics.add(topic_name) + + msg_type = import_msg_type(topic_type) + callback = self._build_topic_callback(topic_cfg) + subscription = self.create_subscription( + msg_type, + topic_name, + callback, + qos_profile_sensor_data, ) - extra = { - "topic": topic_name, - "field": field_name, - "new_value": new, - "prev_value": prev, - } - self.get_logger().info( - f"[Trigger] field_change on '{topic_name}.{field_name}' " - f"from {prev} -> {new} -> event '{evt_type}'" + self.topic_subscriptions.append(subscription) + if role: + self.required_roles[role].append(topic_name) + self.get_logger().info(f"Subscribed to {topic_name} with role '{role}'") + + self._validate_required_roles() + + def _validate_required_roles(self): + required = [ + "autonomous_mode", + "intervention_heartbeat", + "robot_state", + "distance_odometry", + "navigation_area", + "global_path", + "global_pose_odometry", + ] + missing = [role for role in required if not self.required_roles.get(role)] + if missing: + raise RuntimeError( + "Config is missing required topic roles: " + ", ".join(sorted(missing)) ) - self.trigger_intervention(evt_type, extra=extra) - - # ------------------------- - # Callbacks - # ------------------------- - def odom_role_callback(self, topic_name, msg): - try: - pos = msg.pose.pose.position - # Update Snapshot with raw odom info - self.system_snapshot['odometry'] = { - 'x': pos.x, - 'y': pos.y, - 'vx': msg.twist.twist.linear.x - } - except Exception: - return - - current_time = self.get_clock().now() - if self.init_pose: - self.init_pose = False - self.previous_x = pos.x - self.previous_y = pos.y - self.previous_time = current_time - self.last_odom_update_time = current_time - self.get_logger().info( - f"[Odom] Initial pose set x={pos.x:.3f}, y={pos.y:.3f}" - ) - return - - dx = pos.x - self.previous_x - dy = pos.y - self.previous_y - dist = math.sqrt(dx * dx + dy * dy) - - if dist < self.min_distance_threshold: - # Too small to count as movement - return - - if not hasattr(self, 'previous_time'): - self.previous_time = current_time - - time_diff = (current_time - self.previous_time).nanoseconds * 1e-9 - - self.speed = dist / time_diff if time_diff > 0 else 0.0 - self.distance += dist - - # Use observed mode if enabled (fallback when no explicit control_mode topic) - is_moving = self.speed > self.mode_observer_speed_threshold - self.update_observed_mode_from_odom(is_moving=is_moving, now=current_time) - - if self.details.get('operation_mode') == self.AUTO: - self.autonomous_distance += dist - - self.previous_x = pos.x - self.previous_y = pos.y - self.previous_time = current_time - self.last_odom_update_time = current_time - - # Keep metrics in the snapshot too - self.system_snapshot['metrics'] = { - 'distance': self.distance, - 'autonomous_distance': self.autonomous_distance, - 'speed': self.speed, + if len(self.required_roles.get("autonomous_mode", [])) != 1: + raise RuntimeError("Exactly one topic must use role 'autonomous_mode'") + if len(self.required_roles.get("robot_state", [])) != 1: + raise RuntimeError("Exactly one topic must use role 'robot_state'") + if len(self.required_roles.get("distance_odometry", [])) != 1: + raise RuntimeError("Exactly one topic must use role 'distance_odometry'") + if len(self.required_roles.get("global_path", [])) != 1: + raise RuntimeError("Exactly one topic must use role 'global_path'") + if len(self.required_roles.get("global_pose_odometry", [])) != 1: + raise RuntimeError("Exactly one topic must use role 'global_pose_odometry'") + + def _init_session(self): + env_variables = { + "robot_name": os.getenv("ROBOT_NAME", "UNDEFINED"), + "farm_name": os.getenv("FARM_NAME", "UNDEFINED"), + "field_name": os.getenv("FIELD_NAME", "UNDEFINED"), + "application": os.getenv("APPLICATION", "UNDEFINED"), + "scenario_name": os.getenv("SCENARIO_NAME", "UNDEFINED"), + } + git_repos = collect_git_repos_info(self.config) + initial_summary = self._build_summary() + active_db_managers = [] + for db_manager in self.db_managers: + try: + db_manager.init_session( + env_variables=env_variables, + git_repos_info=git_repos, + metadata=initial_summary, + ) + active_db_managers.append(db_manager) + except Exception as exc: + self.get_logger().warn(f"Failed to initialize MongoDB session: {exc}") + self.db_managers = active_db_managers + if not self.db_managers: + self.get_logger().warn("No MongoDB targets are currently writable.") + + def _build_topic_callback(self, topic_cfg: dict): + topic_name = topic_cfg["name"] + role = topic_cfg.get("role", "").strip() + + def callback(msg): + self._store_latest_topic_value(topic_name, role, msg) + if role == "autonomous_mode": + self._handle_autonomous_mode(msg) + elif role == "intervention_heartbeat": + self._handle_intervention_heartbeat(topic_cfg, msg) + elif role == "robot_state": + self._handle_robot_state(msg) + elif role == "distance_odometry": + self._handle_distance_odometry(msg) + elif role == "navigation_area": + self._handle_navigation_area(msg) + elif role == "global_path": + self._handle_global_path(msg) + elif role == "global_pose_odometry": + self._handle_global_pose(msg) + + return callback + + def _store_latest_topic_value(self, topic_name: str, role: str, msg): + topic_cfg = self.topic_cfg_map[topic_name] + data = self._serialize_topic_value(topic_cfg, msg) + self.latest_topic_values[topic_name] = { + "role": role, + "received_at": self._now_datetime().isoformat(), + "data": data, } - self.get_logger().debug( - f"[Odom] step_dist={dist:.3f} m, total_dist={self.distance:.3f} m, " - f"auto_dist={self.autonomous_distance:.3f} m, speed={self.speed:.3f} m/s, " - f"mode={self.details.get('operation_mode')}" - ) - - self.publish_distance(self.distance) - self.publish_speed(self.speed) - self.handle_dynamic_publish(topic_name, msg) - - def control_mode_role_callback(self, topic_name, msg): - cfg = self.topic_cfg_map.get(topic_name, {}) - mode_field = cfg.get('mode_field', 'data') - try: - val = get_nested_field(msg, mode_field) - except Exception: + def _serialize_topic_value(self, topic_cfg: dict, msg): + role = topic_cfg.get("role", "") + if role in {"autonomous_mode", "intervention_heartbeat"}: + return {"value": bool(msg.data)} + if role == "robot_state": + return {"value": str(msg.data)} + if role in {"distance_odometry", "global_pose_odometry"}: + return { + "frame_id": msg.header.frame_id, + "position": { + "x": float(msg.pose.pose.position.x), + "y": float(msg.pose.pose.position.y), + "z": float(msg.pose.pose.position.z), + }, + "linear_speed_mps": float(msg.twist.twist.linear.x), + } + if role == "global_path": + pose_count = len(msg.poses) + data = {"pose_count": pose_count} + if pose_count > 0: + start = msg.poses[0].pose.position + end = msg.poses[-1].pose.position + data["start"] = {"x": float(start.x), "y": float(start.y)} + data["end"] = {"x": float(end.x), "y": float(end.y)} + return data + return ros_msg_to_dict(msg) + + def _handle_autonomous_mode(self, msg: Bool): + new_mode = bool(msg.data) + if new_mode == self.autonomous_mode: return - mapping = cfg.get('mode_mapping', {}) - new_mode = mapping.get(str(val)) or mapping.get(val) - - if new_mode is None: - if str(val) == "3": - new_mode = self.MAN - else: - new_mode = self.AUTO - - current_time = datetime.now() + previous_mode = self.autonomous_mode + failing_heartbeats = self._get_failing_intervention_heartbeats() + self._advance_mode_clock(new_mode) - if new_mode != self.details.get('operation_mode'): - prev_mode = self.details.get('operation_mode') - self.details['operation_mode'] = new_mode - self.get_logger().info( - f"Mode changed (explicit): {prev_mode} -> {new_mode} (raw={val})" - ) - - if prev_mode == self.AUTO and new_mode == self.MAN: - if self.autonomous_start_time: - delta = (current_time - self.autonomous_start_time).total_seconds() - self.autonomous_time += delta - self.autonomous_start_time = None - - # Manual override is an intervention - self.trigger_intervention('Manual_override', force_count=True) - - elif new_mode == self.AUTO: - self.autonomous_start_time = current_time - # Logging autonomous mode change (not counted as intervention) - self.log_event('Autonomous', { - **self.details, - 'system_snapshot': self.system_snapshot.copy() - }) - - def estop_role_callback(self, topic_name, msg): - cfg = self.topic_cfg_map.get(topic_name, {}) - field = cfg.get('field', 'data') - try: - v = bool(get_nested_field(msg, field)) - # Update Snapshot - self.system_snapshot['estop'] = v - except Exception: - return + transition_details = { + "previous_mode": AUTONOMOUS_LABEL if previous_mode else MANUAL_LABEL, + "new_mode": AUTONOMOUS_LABEL if new_mode else MANUAL_LABEL, + "failing_heartbeats": failing_heartbeats, + } + self._log_event("mode_transition", transition_details) - if v != self.details.get('estop', False): - self.details['estop'] = v - self.get_logger().info(f"E-Stop state changed: {v}") - if v: - # E-stop counts as intervention - self.trigger_intervention('EMS') - - def generic_callback(self, topic_name, msg, log_fields): - cfg = self.topic_cfg_map.get(topic_name, {}) - role = cfg.get('role', '').lower() - - # Mode observer special-case - if role == 'mode_observer': - self.last_auto_cmd_time = self.get_clock().now() - self.system_snapshot['mode_observer'] = { - 'topic': topic_name + if previous_mode and not new_mode and failing_heartbeats: + self.intervention_count += 1 + self.last_intervention_details = { + "time": self._now_datetime().isoformat(), + "failing_heartbeats": failing_heartbeats, } - self.get_logger().debug( - f"[ModeObserver] Received command on {topic_name}, " - f"marking recent autonomous command." + self._log_event( + "intervention", + { + "intervention_index": self.intervention_count, + "failing_heartbeats": failing_heartbeats, + }, ) - data = {} - log_all = bool(cfg.get("log_all_fields", False)) - - if log_all: - try: - data = ros_msg_to_dict(msg) - except Exception as e: - self.get_logger().warn( - f"Failed to serialize all fields for {topic_name}: {e}" - ) - data = {} - else: - for f in log_fields: - try: - data[f] = get_nested_field(msg, f) - except Exception: - pass - - if data: - # Update Snapshot with the latest values from this topic - self.system_snapshot[topic_name] = data - - # If this topic carries battery info, update the battery state - battery_field = cfg.get("battery_field") - if battery_field and battery_field in data: - old_batt = self.current_battery - self.current_battery = data[battery_field] - if old_batt is None or abs(self.current_battery - old_batt) >= 1.0: - self.get_logger().info( - f"[Battery] {topic_name}.{battery_field} updated: " - f"{old_batt} -> {self.current_battery}" - ) - - # Apply YAML-configured intervention triggers (message & field-change) - self.handle_intervention_triggers(topic_name, data, cfg) - - # Dynamic republishing if configured - self.handle_dynamic_publish(topic_name, msg) - - def handle_dynamic_publish(self, topic_name, msg): - pub_cfg = self.dynamic_publishers.get(topic_name) - if not pub_cfg: - return - publisher, pub_msg_cls, pub_field = pub_cfg - - try: - val = get_nested_field(msg, pub_field) if pub_field else msg - - out_msg = pub_msg_cls() - if hasattr(out_msg, 'data'): - if 'Float' in pub_msg_cls.__name__: - out_msg.data = float(val) - elif 'Int' in pub_msg_cls.__name__: - out_msg.data = int(val) - elif 'String' in pub_msg_cls.__name__: - out_msg.data = str(val) - else: - out_msg.data = val - else: - # Try simple attribute matching - for attr in dir(out_msg): - if not attr.startswith('_'): - try: - setattr(out_msg, attr, val) - break - except Exception: - continue - - publisher.publish(out_msg) - self.get_logger().debug( - f"[DynamicPublish] topic={topic_name} -> republished " - f"value={val} ({pub_msg_cls.__name__})" + def _handle_intervention_heartbeat(self, topic_cfg: dict, msg: Bool): + heartbeat_key = topic_cfg.get("metric_key") or topic_cfg["name"] + self.intervention_heartbeat_states[heartbeat_key] = bool(msg.data) + + def _handle_robot_state(self, msg: String): + new_state = self._normalize_robot_state(str(msg.data)) + previous_state = self.robot_state + self.robot_state = new_state + + if ( + previous_state == ROBOT_STATE_ACTIVE + and new_state == ROBOT_STATE_DISABLED + and self.autonomous_mode + ): + self.active_to_disabled_in_autonomous_count += 1 + self._log_event( + "robot_state_active_to_disabled_in_autonomous", + { + "count": self.active_to_disabled_in_autonomous_count, + "previous_state": previous_state, + "new_state": new_state, + }, ) - except Exception as e: - self.get_logger().error(f"Dynamic publish failed for {topic_name}: {e}") - - def update_observed_mode_from_odom(self, is_moving: bool, now=None): - """ - Fallback mode inference when there is no explicit /gophar/operation_mode. - - Logic: - - If robot is moving AND we have recent /cmd_vel/collision_smoothed, mode = Autonomous - - If robot is moving AND no recent /cmd_vel/collision_smoothed, mode = Manual - - If robot is not moving, keep previous mode. - - If mode changes from AUTO -> MAN, log an intervention. - """ - if not self.mode_observer_active: + def _handle_distance_odometry(self, msg: Odometry): + now_sec = self._now_sec() + self.last_speed_update_sec = now_sec + self.current_speed_mps = abs(float(msg.twist.twist.linear.x)) + + position = msg.pose.pose.position + current_position = (float(position.x), float(position.y)) + if self.last_motion_position is None: + self.last_motion_position = current_position + self.first_motion_time_sec = now_sec + self.last_motion_time_sec = now_sec return - if now is None: - now = self.get_clock().now() - - prev_mode = self.details.get('operation_mode', self.MAN) - new_mode = prev_mode - - auto_cmd_recent = False - if self.last_auto_cmd_time is not None: - dt = (now - self.last_auto_cmd_time).nanoseconds * 1e-9 - auto_cmd_recent = dt <= self.mode_observer_cmd_timeout + dx = current_position[0] - self.last_motion_position[0] + dy = current_position[1] - self.last_motion_position[1] + step_distance_m = math.hypot(dx, dy) - self.get_logger().debug( - f"[ModeObserver] is_moving={is_moving}, auto_cmd_recent={auto_cmd_recent}, " - f"prev_mode={prev_mode}" - ) + if step_distance_m >= self.distance_epsilon_m: + self.total_distance_m += step_distance_m + self.distance_by_mode_m[self._mode_label()] += step_distance_m + if self.current_area: + self.area_distance_m[self.current_area] += step_distance_m - if not is_moving: - # Don't flip modes just because we stopped; keep previous - return + self.last_motion_position = current_position + self.last_motion_time_sec = now_sec - if auto_cmd_recent: - new_mode = self.AUTO - else: - new_mode = self.MAN + def _handle_navigation_area(self, msg: String): + new_area = str(msg.data).strip() + now_sec = self._now_sec() - if new_mode == prev_mode: + if new_area == self.current_area: return - self.get_logger().info(f"[ModeObserver] Observed mode change: {prev_mode} -> {new_mode}") - - current_time = datetime.now() - - # Track autonomous time like the explicit control_mode callback - if prev_mode == self.AUTO and new_mode == self.MAN: - if self.autonomous_start_time: - delta = (current_time - self.autonomous_start_time).total_seconds() - self.autonomous_time += delta - self.autonomous_start_time = None - - # Observed manual override counts as an intervention - extra = { - "mode_source": "observed", - "observer_topic": self.mode_observer_topic_name, - } - self.trigger_intervention("Manual_override_observed", extra=extra, force_count=True) - - elif prev_mode != self.AUTO and new_mode == self.AUTO: - self.autonomous_start_time = current_time - - self.details['operation_mode'] = new_mode + if self.current_area is not None and self.area_entered_at is not None: + self.area_totals_sec[self.current_area] += now_sec - self.area_entered_at - def collision_nav_callback(self, topic_name, msg): - """ - Store latest /cmd_vel/nav command. - """ - now = self.get_clock().now() - self.last_nav_cmd = msg - self.last_nav_time = now + self.current_area = new_area + self.area_entered_at = now_sec + self.area_visit_count[new_area] += 1 + self._log_event("navigation_area_transition", {"new_area": new_area}) - self.system_snapshot['cmd_vel_nav'] = { - 'linear_x': msg.linear.x, - 'linear_y': msg.linear.y, - 'angular_z': msg.angular.z, - } - - self.get_logger().debug( - f"[Collision] NAV cmd received: vx={msg.linear.x:.3f}, vy={msg.linear.y:.3f}, " - f"wz={msg.angular.z:.3f}" - ) - # IMPORTANT: do NOT call check_collision_condition here - - def collision_output_callback(self, topic_name, msg): - """ - Store latest /cmd_vel/collision command and check for collision condition. - We only increment collision_incidents on the falling edge: - - previously had velocity on /cmd_vel/collision - - now ~zero velocity - - while /cmd_vel/nav still commands forward motion - """ - now = self.get_clock().now() - self.last_collision_cmd = msg - self.last_collision_time = now - - coll_vx = float(msg.linear.x) - - self.system_snapshot['cmd_vel_collision'] = { - 'linear_x': msg.linear.x, - 'linear_y': msg.linear.y, - 'angular_z': msg.angular.z, - } - - self.get_logger().debug( - f"[Collision] COLLISION cmd received: vx={msg.linear.x:.3f}, vy={msg.linear.y:.3f}, " - f"wz={msg.angular.z:.3f}" - ) - - # Check for falling edge-based collision - self.check_collision_condition(now, coll_vx) - - def check_collision_condition(self, now, coll_vx_now: float): - """ - Detect a collision event only on falling edge: - - - previous /cmd_vel/collision had velocity (|vx| > collision_zero_threshold) - - current /cmd_vel/collision has ~zero velocity (|vx| <= collision_zero_threshold) - - latest /cmd_vel/nav.linear.x > collision_nav_threshold - - nav command is recent (within collision_time_window) - - When this condition is met: - - increment collision_incidents once - - log a 'Collision' event with full snapshot - - publish total collision_incidents - - We only increment again if the cycle repeats: - velocity on /cmd_vel/collision -> zero -> velocity -> zero -> ... - """ - # Need a nav command to interpret the collision behaviour - if self.last_nav_cmd is None or self.last_nav_time is None: - self.get_logger().debug( - "[Collision] No NAV cmd yet; cannot evaluate collision condition." + def _handle_global_path(self, msg: PathMsg): + self.current_path = [ + ( + float(pose.pose.position.x), + float(pose.pose.position.y), ) - collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold - self.collision_prev_has_velocity = collision_has_velocity_now - return + for pose in msg.poses + ] - # Check nav recency - dt_nav = (now - self.last_nav_time).nanoseconds * 1e-9 - if dt_nav > self.collision_time_window: - self.get_logger().debug( - f"[Collision] NAV cmd too old ({dt_nav:.3f}s > {self.collision_time_window}s); " - "skipping collision evaluation." - ) - collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold - self.collision_prev_has_velocity = collision_has_velocity_now + def _handle_global_pose(self, msg: Odometry): + if len(self.current_path) < 2: return - nav_vx = float(self.last_nav_cmd.linear.x) - - # Does nav actually request motion? - if nav_vx <= self.collision_nav_threshold: - self.get_logger().debug( - f"[Collision] NAV vx={nav_vx:.3f} <= nav_threshold={self.collision_nav_threshold}; " - "no forward motion requested." + position = msg.pose.pose.position + x = float(position.x) + y = float(position.y) + current_deviation = min( + self._point_to_segment_distance( + x, + y, + self.current_path[index], + self.current_path[index + 1], ) - collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold - self.collision_prev_has_velocity = collision_has_velocity_now - return - - # Edge detection on /cmd_vel/collision - collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold - - self.get_logger().debug( - f"[Collision] prev_has_vel={self.collision_prev_has_velocity}, " - f"now_has_vel={collision_has_velocity_now}, coll_vx_now={coll_vx_now:.3f}, " - f"nav_vx={nav_vx:.3f}" + for index in range(len(self.current_path) - 1) ) - - # Falling edge: had velocity before, now ~zero - if self.collision_prev_has_velocity and not collision_has_velocity_now: - # This is one collision incident - self.collision_incidents += 1 - self.get_logger().info( - f"[Collision] Detected collision incident #{self.collision_incidents} " - f"(nav_vx={nav_vx:.3f}, coll_vx_now={coll_vx_now:.3f})" - ) - self.log_collision_event() - - # Update for next call - self.collision_prev_has_velocity = collision_has_velocity_now - - # ------------------------- - # Timer & Logging - # ------------------------- - def timer_callback(self): - hb = Bool() - hb.data = True - self.heartbeat_publisher.publish(hb) - - now = self.get_clock().now() - time_since_move = (now - self.last_odom_update_time).nanoseconds * 1e-9 - - if time_since_move > self.stop_timeout and self.speed > 0.0: - self.get_logger().info( - f"[Timer] Robot has not moved for {time_since_move:.2f}s; " - "forcing speed to 0.0" + self.current_path_deviation_m = current_deviation + self.path_deviation_sum_m += current_deviation + self.path_deviation_samples += 1 + self.max_path_deviation_m = max(self.max_path_deviation_m, current_deviation) + + def _advance_mode_clock(self, new_mode: bool): + now_sec = self._now_sec() + old_label = self._mode_label() + self.mode_totals_sec[old_label] += now_sec - self.mode_entered_at + self.autonomous_mode = new_mode + self.mode_entered_at = now_sec + + def _mode_label(self) -> str: + return AUTONOMOUS_LABEL if self.autonomous_mode else MANUAL_LABEL + + def _get_mode_totals_sec(self) -> dict: + totals = dict(self.mode_totals_sec) + totals[self._mode_label()] += self._now_sec() - self.mode_entered_at + return totals + + def _get_area_totals_sec(self) -> dict: + totals = dict(self.area_totals_sec) + if self.current_area is not None and self.area_entered_at is not None: + totals[self.current_area] = totals.get(self.current_area, 0.0) + ( + self._now_sec() - self.area_entered_at ) - self.speed = 0.0 - self.publish_speed(0.0) + return totals + + def _get_failing_intervention_heartbeats(self) -> list[str]: + return sorted( + [ + name + for name, is_healthy in self.intervention_heartbeat_states.items() + if is_healthy is False + ] + ) - def update_db_metrics(self): - # MBDI = autonomous_distance / incidents - if self.incidents != 0: - mdbi_val = float(self.autonomous_distance) / float(self.incidents) - else: - # No incidents → use autonomous distance as-is (no division) - mdbi_val = float(self.autonomous_distance) + def _build_summary(self) -> dict: + now_sec = self._now_sec() + mode_times_sec = self._get_mode_totals_sec() + area_times_sec = self._get_area_totals_sec() - self.get_logger().debug( - f"[DB] metrics update: dist={self.distance:.3f}, auto_dist={self.autonomous_distance:.3f}, " - f"incidents={self.incidents}, collisions={self.collision_incidents}, mdbi={mdbi_val:.3f}" - ) + if ( + self.last_speed_update_sec is not None + and now_sec - self.last_speed_update_sec > self.stale_speed_timeout_sec + ): + live_speed_mps = 0.0 + else: + live_speed_mps = self.current_speed_mps - db_managers = [self.db_mgr_local] - if self.db_mgr_remote: - db_managers.append(self.db_mgr_remote) + elapsed_motion_time = 0.0 + if self.first_motion_time_sec is not None and self.last_motion_time_sec is not None: + elapsed_motion_time = max(0.0, self.last_motion_time_sec - self.first_motion_time_sec) - for dbm in db_managers: - try: - dbm.update_distance(self.distance) - dbm.update_incidents(self.incidents) - dbm.update_autonomous_distance(self.autonomous_distance) - dbm.update_mdbi(mdbi_val) - dbm.update_collision_incidents(self.collision_incidents) - except Exception as e: - self.get_logger().warn(f"DB update failed: {e}") - - def log_event(self, msg='', details=None): - if details is None: - details = {} - - # Attach metrics to every event - details = { - **details, - 'metrics': self.get_metrics_snapshot() - } + average_speed_mps = ( + self.total_distance_m / elapsed_motion_time if elapsed_motion_time > 0.0 else 0.0 + ) - event_time = datetime.now(tz=timezone.utc) - event = {'time': event_time, 'event_type': msg, 'details': details} + if self.intervention_count > 0: + mdbi_m = self.distance_by_mode_m[AUTONOMOUS_LABEL] / self.intervention_count + else: + mdbi_m = None + + area_metrics = {} + for area_name in sorted(set(KNOWN_AREAS).union(area_times_sec).union(self.area_distance_m)): + total_time = float(area_times_sec.get(area_name, 0.0)) + total_distance = float(self.area_distance_m.get(area_name, 0.0)) + visit_count = int(self.area_visit_count.get(area_name, 0)) + area_metrics[area_name] = { + "total_time_sec": total_time, + "average_time_per_visit_sec": total_time / visit_count if visit_count else 0.0, + "distance_m": total_distance, + "average_speed_mps": total_distance / total_time if total_time > 0.0 else 0.0, + "visits": visit_count, + } - self.get_logger().info( - f"[Event] type={msg}, time={event_time.isoformat()}, " - f"incidents={self.incidents}, collisions={self.collision_incidents}" + path_deviation_average_m = ( + self.path_deviation_sum_m / self.path_deviation_samples + if self.path_deviation_samples + else 0.0 ) - self.db_mgr_local.add_event(event) - if self.db_mgr_remote: - try: - self.db_mgr_remote.add_event(event) - except Exception as e: - self.get_logger().warn(f"Remote DB event log failed: {e}") - - incidents_msg = Int32() - incidents_msg.data = self.incidents - self.incidents_publisher.publish(incidents_msg) - - self.update_db_metrics() - - def log_collision_event(self): - """ - Log a 'Collision' event, capturing: - - full system snapshot - - nav & collision commands - - total incidents and collision_incidents - """ - details = { - "collision_incident_index": self.collision_incidents, - "incidents": self.incidents, - "collision_incidents": self.collision_incidents, - "total_incidents": self.incidents + self.collision_incidents, - "nav_cmd": { - "linear": { - "x": float(self.last_nav_cmd.linear.x), - "y": float(self.last_nav_cmd.linear.y), + return { + "timestamp": self._now_datetime().isoformat(), + "mode": { + "autonomous": self.autonomous_mode, + "label": self._mode_label(), + "time_sec": { + AUTONOMOUS_LABEL: float(mode_times_sec[AUTONOMOUS_LABEL]), + MANUAL_LABEL: float(mode_times_sec[MANUAL_LABEL]), }, - "angular": { - "z": float(self.last_nav_cmd.angular.z), + "distance_m": { + AUTONOMOUS_LABEL: float(self.distance_by_mode_m[AUTONOMOUS_LABEL]), + MANUAL_LABEL: float(self.distance_by_mode_m[MANUAL_LABEL]), }, }, - "collision_cmd": { - "linear": { - "x": float(self.last_collision_cmd.linear.x), - "y": float(self.last_collision_cmd.linear.y), - }, - "angular": { - "z": float(self.last_collision_cmd.angular.z), - }, + "interventions": { + "count": int(self.intervention_count), + "failing_heartbeats": self._get_failing_intervention_heartbeats(), + "last": self.last_intervention_details, + }, + "mdbi_m": mdbi_m, + "mdbi_status": "defined" if mdbi_m is not None else "undefined_until_first_intervention", + "robot_state": { + "current": self.robot_state, + "active_to_disabled_in_autonomous_count": int( + self.active_to_disabled_in_autonomous_count + ), + }, + "motion": { + "total_distance_m": float(self.total_distance_m), + "current_speed_mps": float(live_speed_mps), + "average_speed_mps": float(average_speed_mps), + }, + "navigation_area": { + "current": self.current_area, + "states": area_metrics, + }, + "path_deviation": { + "current_m": float(self.current_path_deviation_m or 0.0), + "average_m": float(path_deviation_average_m), + "max_m": float(self.max_path_deviation_m), + "samples": int(self.path_deviation_samples), + }, + "heartbeats": { + key: value for key, value in sorted(self.intervention_heartbeat_states.items()) }, - # Full snapshot of all system data at this moment - "system_snapshot": self.system_snapshot.copy(), } - self.get_logger().info( - f"[Collision] Logging collision event #{self.collision_incidents} " - f"(total_incidents={details['total_incidents']})" - ) - - # This will attach metrics (distance, speed, battery, etc.) and write to DB - self.log_event("Collision", details) - - # Publish total collisions on a dedicated topic - msg = Int32() - msg.data = int(self.collision_incidents) - self.collision_incidents_publisher.publish(msg) - - def publish_distance(self, dist): - msg = Float32() - msg.data = float(dist) - self.distance_publisher.publish(msg) - self.get_logger().debug(f"[Publish] distance={dist:.3f}") - self.update_db_metrics() - - def publish_speed(self, speed): - msg = Float32() - msg.data = float(speed) - self.speed_publisher.publish(msg) - self.get_logger().debug(f"[Publish] speed={speed:.3f}") - - def get_git_info(self, repo_path): - """ - Return basic git metadata for a repo. - - Example schema: - { - "path": "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario", - "exists": True, - "remote": "git@github.com:LCAS/aoc_strawberry_scenario.git", - "branch": "main", - "commit": "f3a6c1b4e6f8c3b3c823f2c45e56a123456789ab", - "short_commit": "f3a6c1b", - "commit_message": "Fix navsat transform delay", - "tags": ["v0.3.1"], - "describe": "v0.3.1-2-gf3a6c1b", - "dirty": false, - "error": null + def _build_snapshot(self) -> dict: + topics = { + topic_name: self.latest_topic_values.get(topic_name) + for topic_name in sorted(self.snapshot_topics) + if topic_name in self.latest_topic_values } - """ - info = { - "path": repo_path, - "exists": False, - "remote": None, - "branch": None, - "commit": None, - "short_commit": None, - "commit_message": None, # NEW - "tags": [], # NEW - "describe": None, # NEW - "dirty": None, - "error": None, + return { + "time": self._now_datetime(), + "summary": self._build_summary(), + "topics": topics, } - try: - if not repo_path or not os.path.isdir(repo_path): - info["error"] = "Path does not exist or is not a directory" - return info - - def git(args): - result = subprocess.run( - ["git", "-C", repo_path] + args, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE, - text=True, - ) - if result.returncode != 0: - raise RuntimeError(result.stderr.strip()) - return result.stdout.strip() - - info["exists"] = True - - # Remote - try: - info["remote"] = git(["config", "--get", "remote.origin.url"]) - except Exception: - info["remote"] = None - - # Branch - try: - info["branch"] = git(["rev-parse", "--abbrev-ref", "HEAD"]) - except Exception: - info["branch"] = None - - # Commit hashes - try: - info["commit"] = git(["rev-parse", "HEAD"]) - except Exception: - info["commit"] = None + def _log_event(self, event_type: str, details: dict): + event = { + "time": self._now_datetime(), + "event_type": event_type, + "details": details, + "summary": self._build_summary(), + } + for db_manager in self.db_managers: try: - info["short_commit"] = git(["rev-parse", "--short", "HEAD"]) - except Exception: - info["short_commit"] = None - - # NEW: Last commit message (full body or just subject) + db_manager.add_event(event) + db_manager.update_session_summary(event["summary"]) + except Exception as exc: + self.get_logger().warn(f"Failed to write event '{event_type}' to MongoDB: {exc}") + + def _snapshot_timer_callback(self): + snapshot = self._build_snapshot() + for db_manager in self.db_managers: try: - info["commit_message"] = git(["log", "-1", "--pretty=%s"]) - except Exception: - info["commit_message"] = None + db_manager.add_snapshot(snapshot) + db_manager.update_session_summary(snapshot["summary"]) + except Exception as exc: + self.get_logger().warn(f"Failed to write periodic snapshot to MongoDB: {exc}") + + def _heartbeat_timer_callback(self): + summary = self._build_summary() + + heartbeat_msg = Bool() + heartbeat_msg.data = True + self.heartbeat_publisher.publish(heartbeat_msg) + + mode_msg = String() + mode_msg.data = summary["mode"]["label"] + self.mode_publisher.publish(mode_msg) + + interventions_msg = Int32() + interventions_msg.data = summary["interventions"]["count"] + self.interventions_publisher.publish(interventions_msg) + + mdbi_msg = Float32() + mdbi_msg.data = float(summary["mdbi_m"]) if summary["mdbi_m"] is not None else float("nan") + self.mdbi_publisher.publish(mdbi_msg) + + transition_msg = Int32() + transition_msg.data = summary["robot_state"][ + "active_to_disabled_in_autonomous_count" + ] + self.robot_state_transition_publisher.publish(transition_msg) + + self._publish_float(self.total_distance_publisher, summary["motion"]["total_distance_m"]) + self._publish_float( + self.autonomous_distance_publisher, + summary["mode"]["distance_m"][AUTONOMOUS_LABEL], + ) + self._publish_float( + self.manual_distance_publisher, + summary["mode"]["distance_m"][MANUAL_LABEL], + ) + self._publish_float( + self.current_speed_publisher, summary["motion"]["current_speed_mps"] + ) + self._publish_float( + self.average_speed_publisher, summary["motion"]["average_speed_mps"] + ) + self._publish_float( + self.autonomous_time_publisher, summary["mode"]["time_sec"][AUTONOMOUS_LABEL] + ) + self._publish_float( + self.manual_time_publisher, summary["mode"]["time_sec"][MANUAL_LABEL] + ) - # NEW: Tags pointing at HEAD - try: - tags_str = git(["tag", "--points-at", "HEAD"]) - if tags_str: - info["tags"] = tags_str.splitlines() - else: - info["tags"] = [] - except Exception: - info["tags"] = [] - - # NEW: git describe (nearest tag + distance) - try: - info["describe"] = git(["describe", "--tags", "--always"]) - except Exception: - info["describe"] = None + area_msg = String() + area_msg.data = self.current_area or "UNKNOWN" + self.navigation_area_publisher.publish(area_msg) - # Dirty / clean state - try: - status = git(["status", "--porcelain"]) - info["dirty"] = bool(status) - except Exception: - info["dirty"] = None + self._publish_float( + self.path_deviation_current_publisher, + summary["path_deviation"]["current_m"], + ) + self._publish_float( + self.path_deviation_average_publisher, + summary["path_deviation"]["average_m"], + ) + self._publish_float( + self.path_deviation_max_publisher, summary["path_deviation"]["max_m"] + ) - except Exception as e: - info["error"] = str(e) + summary_msg = String() + summary_msg.data = json.dumps(summary, sort_keys=True) + self.summary_publisher.publish(summary_msg) - return info + def _publish_float(self, publisher, value: float): + msg = Float32() + msg.data = float(value) + publisher.publish(msg) + + def _normalize_robot_state(self, raw_state: str) -> str: + stripped = raw_state.strip() + lowered = stripped.lower() + if lowered in {"disable", "disabled"}: + return ROBOT_STATE_DISABLED + if lowered == "active": + return ROBOT_STATE_ACTIVE + if lowered == "enabled": + return "Enabled" + return stripped or "Unknown" + + def _now_datetime(self) -> datetime: + return datetime.now(tz=timezone.utc) + + def _now_sec(self) -> float: + return self.get_clock().now().nanoseconds * 1e-9 + + @staticmethod + def _point_to_segment_distance(px, py, start, end) -> float: + start_x, start_y = start + end_x, end_y = end + delta_x = end_x - start_x + delta_y = end_y - start_y + segment_length_sq = delta_x * delta_x + delta_y * delta_y + if segment_length_sq == 0.0: + return math.hypot(px - start_x, py - start_y) + projection = max( + 0.0, + min( + 1.0, + ((px - start_x) * delta_x + (py - start_y) * delta_y) / segment_length_sq, + ), + ) + proj_x = start_x + projection * delta_x + proj_y = start_y + projection * delta_y + return math.hypot(px - proj_x, py - proj_y) def main(args=None): @@ -1089,17 +759,19 @@ def main(args=None): node = AutonomyMetricsLogger() try: rclpy.spin(node) - except KeyboardInterrupt: - node.get_logger().info("Shutting down AutonomyMetricsLogger (KeyboardInterrupt).") + except (KeyboardInterrupt, ExternalShutdownException): pass finally: - node.get_logger().info( - f"Final stats: distance={node.distance:.3f}, auto_distance={node.autonomous_distance:.3f}, " - f"incidents={node.incidents}, collisions={node.collision_incidents}" - ) - node.destroy_node() - rclpy.shutdown() + final_summary = node._build_summary() + for db_manager in node.db_managers: + try: + db_manager.mark_session_end(final_summary) + except Exception as exc: + node.get_logger().warn(f"Failed to mark session end in MongoDB: {exc}") + if node.context.ok(): + node.destroy_node() + rclpy.shutdown() -if __name__ == '__main__': - main() \ No newline at end of file +if __name__ == "__main__": + main() diff --git a/src/autonomy_metrics/autonomy_metrics/topic_utils.py b/src/autonomy_metrics/autonomy_metrics/topic_utils.py new file mode 100644 index 0000000..ce20cd9 --- /dev/null +++ b/src/autonomy_metrics/autonomy_metrics/topic_utils.py @@ -0,0 +1,64 @@ +"""Utility helpers for ROS topic configuration and message serialization.""" + +from __future__ import annotations + +from array import array +from importlib import import_module +import math + + +def import_msg_type(type_str: str): + """Import a ROS message class from ``pkg/msg/Type`` or ``pkg/Type``.""" + parts = type_str.split("/") + if len(parts) == 2: + package_name, message_name = parts + submodule = "msg" + elif len(parts) == 3: + package_name, submodule, message_name = parts + else: + raise ValueError( + "Message type must be 'pkg/msg/Type' or 'pkg/Type', " + f"got: {type_str}" + ) + + module = import_module(f"{package_name}.{submodule}") + return getattr(module, message_name) + + +def ros_msg_to_dict(value): + """Convert a ROS 2 Python message into JSON/Mongo-friendly primitives.""" + if value is None: + return None + + if isinstance(value, (bool, int, str)): + return value + + if isinstance(value, float): + if math.isfinite(value): + return value + return None + + if isinstance(value, bytes): + return value.hex() + + if isinstance(value, array): + return [ros_msg_to_dict(item) for item in value] + + if isinstance(value, (list, tuple)): + return [ros_msg_to_dict(item) for item in value] + + if hasattr(value, "get_fields_and_field_types"): + result = {} + for field_name in value.get_fields_and_field_types().keys(): + result[field_name] = ros_msg_to_dict(getattr(value, field_name)) + return result + + if hasattr(value, "__dict__"): + result = {} + for key, item in value.__dict__.items(): + if key.startswith("_"): + continue + result[key] = ros_msg_to_dict(item) + return result + + return str(value) diff --git a/src/autonomy_metrics/config/metrics_full.yaml b/src/autonomy_metrics/config/metrics_full.yaml index c210be9..1f914ef 100644 --- a/src/autonomy_metrics/config/metrics_full.yaml +++ b/src/autonomy_metrics/config/metrics_full.yaml @@ -1,5 +1,6 @@ +git_repos_base_path: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" + git_repos: - # all repos are with reference to /home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario scenario: "." autonomy_metrics_logger: "jabas/autonomy_metrics_logger" navfn_planner: "jabas/navfn_planner" @@ -11,117 +12,61 @@ git_repos: ppl_gofar_platform: "robots/ppl_gofar_platform" topological_navigation: "contrib/topological_navigation" sentor: "contrib/sentor" - kiss-icp: "contrib/kiss-icp" - -topics: - # --------------------------------------------------------- - # GPS - # --------------------------------------------------------- - - name: "/gps_base/fix" - type: "sensor_msgs/msg/NavSatFix" - log_all_fields: true - - # --------------------------------------------------------- - # Odometry - # --------------------------------------------------------- - - name: "/gophar_vehicle_controller/odometry" - type: "nav_msgs/msg/Odometry" - role: "odometry" - log_fields: - - "pose.pose.position.x" - - "pose.pose.position.y" - - "twist.twist.linear.x" - publish: - enable: false - - # --------------------------------------------------------- - # System Status (intervention on fault) - # --------------------------------------------------------- - - name: "/gophar/system_status" - type: "dynium_gophar_interfaces/msg/SystemStatus" - role: "system_status" - log_all_fields: true + kiss_icp: "contrib/kiss-icp" - # Tell logger which field carries battery level so it can be attached - battery_field: "battery_percentage" +runtime: + heartbeat_topic: "/autonomy_metrics/heartbeat" + session_summary_topic: "/autonomy_metrics/session_summary_json" + heartbeat_period_sec: 1.0 + snapshot_interval_sec: 10.0 + distance_epsilon_m: 0.01 + stale_speed_timeout_sec: 2.0 - publish: - enable: true - topic: "mdbi_logger/battery_level" - type: "std_msgs/msg/Float32" - field: "battery_percentage" - - # Trigger an intervention when 'fault_shutdown' changes to True - intervention_on_change: - fault_shutdown: - trigger_value: true # Only on False -> True edge - event_type: "Fault_shutdown" - - # --------------------------------------------------------- - # [MISSING] Control Mode (REQUIRED FOR MDBI) - # --------------------------------------------------------- - # You must add this so the logger knows when to count 'autonomous' distance. - # Adjust 'name' and 'mode_mapping' to match your robot. - # - name: "/gophar/operation_mode" # <--- CHECK THIS TOPIC NAME - # type: "std_msgs/msg/String" # <--- CHECK THIS TYPE (String, Int8, etc) - # role: "control_mode" - # mode_field: "data" # The field inside the message holding the mode - # # Map the values your robot sends to 'Autonomous' or 'Manual' - # mode_mapping: - # "AUTO": "Autonomous" - # "MANUAL": "Manual" - # "1": "Autonomous" - # "0": "Manual" +topics: + - name: "/autonomous_mode" + type: "std_msgs/msg/Bool" + role: "autonomous_mode" + snapshot: true - # --------------------------------------------------------- - # Mode observation via velocity command - # --------------------------------------------------------- - # Rule: the heuristic will only be used if there is no topic with role: "control_mode" configured in the YAML. - - name: "/cmd_vel/collision_smoothed" - type: "geometry_msgs/msg/Twist" - role: "mode_observer" - log_fields: [] # keep empty if you don't want per-message events + - name: "/autonomy_checks/heartbeat" + type: "std_msgs/msg/Bool" + role: "intervention_heartbeat" + metric_key: "autonomy_checks" + snapshot: true + - name: "/navigation_safe/heartbeat" + type: "std_msgs/msg/Bool" + role: "intervention_heartbeat" + metric_key: "navigation_safe" + snapshot: true - # --------------------------------------------------------- - # Joystick command -> Intervention trigger on activity - # --------------------------------------------------------- - - name: "/cmd_vel_joy_smoothed" - type: "geometry_msgs/msg/Twist" # Adjust if different - log_fields: [] # We don't need to log individual fields here - # When any message arrives on this topic, treat as an intervention - intervention_on_message: - enable: true - event_type: "Joy_override" + - name: "/system_check/heartbeat" + type: "std_msgs/msg/Bool" + role: "intervention_heartbeat" + metric_key: "system_check" + snapshot: true - # --------------------------------------------------------- - # Collision monitor: nav vs collision-filtered velocity - # --------------------------------------------------------- - - name: "/cmd_vel/nav" - type: "geometry_msgs/msg/Twist" - role: "collision_nav" - log_fields: [] # no per-message events needed + - name: "/robot_state" + type: "std_msgs/msg/String" + role: "robot_state" + snapshot: true - - name: "/cmd_vel/collision" - type: "geometry_msgs/msg/Twist" - role: "collision_output" - log_fields: [] # no per-message events needed + - name: "/gophar_vehicle_controller/odometry" + type: "nav_msgs/msg/Odometry" + role: "distance_odometry" + snapshot: true - # --------------------------------------------------------- - # Steering actuator + motor status snapshots for interventions - # --------------------------------------------------------- - - name: "/gophar/steering_actuator/front" - type: "dynium_gophar_interfaces/msg/SteeringActuatorStatus" - log_all_fields: true - - - name: "/gophar/steering_actuator/rear" - type: "dynium_gophar_interfaces/msg/SteeringActuatorStatus" - log_all_fields: true + - name: "/robot_navigation_area" + type: "std_msgs/msg/String" + role: "navigation_area" + snapshot: true - - name: "/gophar/motor_status/motor_0" - type: "dynium_gophar_interfaces/msg/MotorStatus" - log_all_fields: true + - name: "/plan" + type: "nav_msgs/msg/Path" + role: "global_path" + snapshot: true - - name: "/gophar/motor_status/motor_1" - type: "dynium_gophar_interfaces/msg/MotorStatus" - log_all_fields: true \ No newline at end of file + - name: "/odometry/global" + type: "nav_msgs/msg/Odometry" + role: "global_pose_odometry" + snapshot: true diff --git a/src/autonomy_metrics/config/metrics_test.yaml b/src/autonomy_metrics/config/metrics_test.yaml new file mode 100644 index 0000000..7b566c9 --- /dev/null +++ b/src/autonomy_metrics/config/metrics_test.yaml @@ -0,0 +1,62 @@ +git_repos_base_path: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" + +git_repos: + scenario: "." + autonomy_metrics_logger: "jabas/autonomy_metrics_logger" + +runtime: + heartbeat_topic: "/autonomy_metrics/heartbeat" + session_summary_topic: "/autonomy_metrics/session_summary_json" + heartbeat_period_sec: 0.5 + snapshot_interval_sec: 2.0 + distance_epsilon_m: 0.01 + stale_speed_timeout_sec: 1.0 + +topics: + - name: "/autonomous_mode" + type: "std_msgs/msg/Bool" + role: "autonomous_mode" + snapshot: true + + - name: "/autonomy_checks/heartbeat" + type: "std_msgs/msg/Bool" + role: "intervention_heartbeat" + metric_key: "autonomy_checks" + snapshot: true + + - name: "/navigation_safe/heartbeat" + type: "std_msgs/msg/Bool" + role: "intervention_heartbeat" + metric_key: "navigation_safe" + snapshot: true + + - name: "/system_check/heartbeat" + type: "std_msgs/msg/Bool" + role: "intervention_heartbeat" + metric_key: "system_check" + snapshot: true + + - name: "/robot_state" + type: "std_msgs/msg/String" + role: "robot_state" + snapshot: true + + - name: "/gophar_vehicle_controller/odometry" + type: "nav_msgs/msg/Odometry" + role: "distance_odometry" + snapshot: true + + - name: "/robot_navigation_area" + type: "std_msgs/msg/String" + role: "navigation_area" + snapshot: true + + - name: "/plan" + type: "nav_msgs/msg/Path" + role: "global_path" + snapshot: true + + - name: "/odometry/global" + type: "nav_msgs/msg/Odometry" + role: "global_pose_odometry" + snapshot: true diff --git a/src/autonomy_metrics/launch/autonomy_metrics.launch.py b/src/autonomy_metrics/launch/autonomy_metrics.launch.py index 91f5984..9df6ea9 100644 --- a/src/autonomy_metrics/launch/autonomy_metrics.launch.py +++ b/src/autonomy_metrics/launch/autonomy_metrics.launch.py @@ -1,167 +1,71 @@ #!/usr/bin/env python3 import os + +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - # ------------------------------------------------------------------------- - # Launch arguments (all node parameters exposed) - # ------------------------------------------------------------------------- - - pkg_dir = get_package_share_directory('autonomy_metrics') - - config_yaml_arg = DeclareLaunchArgument( - 'config_yaml', - default_value=os.path.join(pkg_dir, 'config', 'metrics_full.yaml'), - description='Path to the YAML config file for AutonomyMetricsLogger', - ) - - mongodb_host_arg = DeclareLaunchArgument( - 'mongodb_host', - default_value='localhost', - description='Hostname of local MongoDB instance', - ) - - mongodb_port_arg = DeclareLaunchArgument( - 'mongodb_port', - default_value='27018', - description='Port of local MongoDB instance', - ) - - remote_mongodb_host_arg = DeclareLaunchArgument( - 'remote_mongodb_host', - default_value='', - description='Hostname of remote MongoDB instance (leave empty to disable)', - ) - - remote_mongodb_port_arg = DeclareLaunchArgument( - 'remote_mongodb_port', - default_value='27017', - description='Port of remote MongoDB instance', - ) - - enable_remote_logging_arg = DeclareLaunchArgument( - 'enable_remote_logging', - default_value='false', - description='Enable logging to remote MongoDB (true/false)', - ) - - min_distance_threshold_arg = DeclareLaunchArgument( - 'min_distance_threshold', - default_value='0.2', - description='Minimum odom distance increment (m) required to update metrics', - ) - - stop_timeout_arg = DeclareLaunchArgument( - 'stop_timeout', - default_value='2.0', - description='Timeout (s) after last odom movement before speed is forced to 0', - ) - - # Mode observer (velocity-based control mode inference) - mode_observer_cmd_timeout_arg = DeclareLaunchArgument( - 'mode_observer_cmd_timeout', - default_value='1.0', - description='Time window (s) in which a cmd_vel command is considered recent for mode observer', - ) - - mode_observer_speed_threshold_arg = DeclareLaunchArgument( - 'mode_observer_speed_threshold', - default_value='0.01', - description='Speed (m/s) above which robot is considered moving for mode observer', - ) - # Collision monitor (nav vs collision output) - collision_nav_threshold_arg = DeclareLaunchArgument( - 'collision_nav_threshold', - default_value='0.01', - description='Minimum nav linear.x to consider a forward motion command', - ) - - collision_zero_threshold_arg = DeclareLaunchArgument( - 'collision_zero_threshold', - default_value='0.001', - description='Absolute linear.x threshold below which collision output is considered zero', - ) - - collision_time_window_arg = DeclareLaunchArgument( - 'collision_time_window', - default_value='0.5', - description='Max age (s) of nav/collision commands to consider for collision detection', - ) - collision_log_cooldown_arg = DeclareLaunchArgument( - 'collision_log_cooldown', - default_value='1.0', - description='Cooldown (s) between consecutive collision logs (if you still use cooldown)', - ) - - # ------------------------------------------------------------------------- - # LaunchConfigurations (bind arguments to parameters) - # ------------------------------------------------------------------------- - config_yaml = LaunchConfiguration('config_yaml') - mongodb_host = LaunchConfiguration('mongodb_host') - mongodb_port = LaunchConfiguration('mongodb_port') - remote_mongodb_host = LaunchConfiguration('remote_mongodb_host') - remote_mongodb_port = LaunchConfiguration('remote_mongodb_port') - enable_remote_logging = LaunchConfiguration('enable_remote_logging') - min_distance_threshold = LaunchConfiguration('min_distance_threshold') - stop_timeout = LaunchConfiguration('stop_timeout') - - mode_observer_cmd_timeout = LaunchConfiguration('mode_observer_cmd_timeout') - mode_observer_speed_threshold = LaunchConfiguration('mode_observer_speed_threshold') - - collision_nav_threshold = LaunchConfiguration('collision_nav_threshold') - collision_zero_threshold = LaunchConfiguration('collision_zero_threshold') - collision_time_window = LaunchConfiguration('collision_time_window') - collision_log_cooldown = LaunchConfiguration('collision_log_cooldown') - - # ------------------------------------------------------------------------- - # AutonomyMetricsLogger node - # ------------------------------------------------------------------------- - metrics_logger_node = Node( - package='autonomy_metrics', - executable='metric_logger', - name='mdbi_logger_dynamic', - output='screen', - parameters=[{ - 'config_yaml': config_yaml, - 'mongodb_host': mongodb_host, - 'mongodb_port': mongodb_port, - 'remote_mongodb_host': remote_mongodb_host, - 'remote_mongodb_port': remote_mongodb_port, - 'enable_remote_logging': enable_remote_logging, - 'min_distance_threshold': min_distance_threshold, - 'stop_timeout': stop_timeout, - - 'mode_observer_cmd_timeout': mode_observer_cmd_timeout, - 'mode_observer_speed_threshold': mode_observer_speed_threshold, - - 'collision_nav_threshold': collision_nav_threshold, - 'collision_zero_threshold': collision_zero_threshold, - 'collision_time_window': collision_time_window, - 'collision_log_cooldown': collision_log_cooldown, - }], +def generate_launch_description(): + metrics_pkg_dir = get_package_share_directory("autonomy_metrics") + + return LaunchDescription( + [ + DeclareLaunchArgument( + "config_yaml", + default_value=os.path.join(metrics_pkg_dir, "config", "metrics_full.yaml"), + ), + DeclareLaunchArgument("mongodb_host", default_value="localhost"), + DeclareLaunchArgument("mongodb_port", default_value="27018"), + DeclareLaunchArgument("remote_mongodb_host", default_value=""), + DeclareLaunchArgument("remote_mongodb_port", default_value="27017"), + DeclareLaunchArgument("enable_remote_logging", default_value="false"), + DeclareLaunchArgument("database_name", default_value="robot_incidents"), + DeclareLaunchArgument("use_webui", default_value="true"), + DeclareLaunchArgument("ui_host", default_value="0.0.0.0"), + DeclareLaunchArgument("ui_port", default_value="8081"), + DeclareLaunchArgument("ui_access_log", default_value="false"), + DeclareLaunchArgument("prefer_remote_data", default_value="false"), + Node( + package="autonomy_metrics", + executable="metric_logger", + name="autonomy_metrics_logger", + output="screen", + parameters=[ + { + "config_yaml": LaunchConfiguration("config_yaml"), + "mongodb_host": LaunchConfiguration("mongodb_host"), + "mongodb_port": LaunchConfiguration("mongodb_port"), + "remote_mongodb_host": LaunchConfiguration("remote_mongodb_host"), + "remote_mongodb_port": LaunchConfiguration("remote_mongodb_port"), + "enable_remote_logging": LaunchConfiguration("enable_remote_logging"), + "database_name": LaunchConfiguration("database_name"), + } + ], + ), + Node( + package="autonomy_metrics_webui", + executable="autonomy_metrics_webui_server", + name="autonomy_metrics_webui_server", + output="screen", + condition=IfCondition(LaunchConfiguration("use_webui")), + parameters=[ + { + "ui_host": LaunchConfiguration("ui_host"), + "ui_port": LaunchConfiguration("ui_port"), + "ui_access_log": LaunchConfiguration("ui_access_log"), + "mongodb_host": LaunchConfiguration("mongodb_host"), + "mongodb_port": LaunchConfiguration("mongodb_port"), + "remote_mongodb_host": LaunchConfiguration("remote_mongodb_host"), + "remote_mongodb_port": LaunchConfiguration("remote_mongodb_port"), + "prefer_remote_data": LaunchConfiguration("prefer_remote_data"), + "database_name": LaunchConfiguration("database_name"), + } + ], + ), + ] ) - - return LaunchDescription([ - config_yaml_arg, - mongodb_host_arg, - mongodb_port_arg, - remote_mongodb_host_arg, - remote_mongodb_port_arg, - enable_remote_logging_arg, - min_distance_threshold_arg, - stop_timeout_arg, - mode_observer_cmd_timeout_arg, - mode_observer_speed_threshold_arg, - collision_nav_threshold_arg, - collision_zero_threshold_arg, - collision_time_window_arg, - collision_log_cooldown_arg, - metrics_logger_node, - ]) diff --git a/src/autonomy_metrics/package.xml b/src/autonomy_metrics/package.xml index 52397cb..63642af 100644 --- a/src/autonomy_metrics/package.xml +++ b/src/autonomy_metrics/package.xml @@ -2,16 +2,19 @@ autonomy_metrics - 0.0.0 - A centralized package provides a logging node designed to track and record all operational events of robots + 0.1.0 + Config-driven autonomy metrics logger with MongoDB session logging and operational robustness metrics. Ibrahim Apache 2.0 - sensor_msgs rclpy std_msgs nav_msgs - + ament_index_python + python3-pymongo + python3-yaml + launch + launch_ros ament_python diff --git a/src/autonomy_metrics/setup.py b/src/autonomy_metrics/setup.py index b7914ac..308c200 100644 --- a/src/autonomy_metrics/setup.py +++ b/src/autonomy_metrics/setup.py @@ -5,20 +5,20 @@ setup( name=package_name, - version='0.0.0', + version='0.1.0', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), ('share/' + package_name + '/config/', glob('config/*', recursive=True)), - ('share/' + package_name + '/launch/', glob('launch/*', recursive=True)), + ('share/' + package_name + '/launch/', glob('launch/*.py')), ], - install_requires=['setuptools'], + install_requires=['setuptools', 'pymongo', 'PyYAML'], zip_safe=True, - maintainer='ibrahim hroob', + maintainer='Ibrahim', maintainer_email='ibrahim.hroub7@gmail.com', - description='TODO: Package description', + description='Config-driven autonomy metrics logger with MongoDB-backed session snapshots.', license='Apache 2.0', tests_require=['pytest'], entry_points={ diff --git a/src/autonomy_metrics_webui/autonomy_metrics_webui/__init__.py b/src/autonomy_metrics_webui/autonomy_metrics_webui/__init__.py new file mode 100644 index 0000000..7cd6054 --- /dev/null +++ b/src/autonomy_metrics_webui/autonomy_metrics_webui/__init__.py @@ -0,0 +1 @@ +"""Autonomy metrics web UI package.""" diff --git a/src/autonomy_metrics_webui/autonomy_metrics_webui/static/dashboard.css b/src/autonomy_metrics_webui/autonomy_metrics_webui/static/dashboard.css new file mode 100644 index 0000000..e8f0674 --- /dev/null +++ b/src/autonomy_metrics_webui/autonomy_metrics_webui/static/dashboard.css @@ -0,0 +1,276 @@ +:root { + --bg: #0b1114; + --bg-elevated: #111b20; + --panel: rgba(20, 33, 39, 0.9); + --line: rgba(168, 190, 196, 0.15); + --text: #eef4f3; + --muted: #9fb4b1; + --accent: #d18c23; + --accent-soft: rgba(209, 140, 35, 0.16); + --good: #3ca670; + --good-soft: rgba(60, 166, 112, 0.15); + --bad: #d9544f; + --bad-soft: rgba(217, 84, 79, 0.16); +} + +* { + box-sizing: border-box; +} + +body { + margin: 0; + min-height: 100vh; + background: + radial-gradient(circle at top left, rgba(209, 140, 35, 0.12), transparent 28%), + linear-gradient(180deg, #091014 0%, #0d171b 100%); + color: var(--text); + font-family: "IBM Plex Sans", "Segoe UI", sans-serif; +} + +.shell { + max-width: 1460px; + margin: 0 auto; + padding: 28px; +} + +.hero { + display: flex; + justify-content: space-between; + gap: 24px; + align-items: flex-end; + margin-bottom: 24px; +} + +.eyebrow { + margin: 0 0 10px; + text-transform: uppercase; + letter-spacing: 0.16em; + color: var(--accent); + font-size: 0.78rem; +} + +.hero h1 { + margin: 0; + font-size: clamp(2rem, 3vw, 3rem); + line-height: 1.05; +} + +.subhead { + max-width: 760px; + margin: 12px 0 0; + color: var(--muted); + line-height: 1.6; +} + +.status-panel { + min-width: 280px; + display: flex; + flex-direction: column; + align-items: flex-end; + gap: 12px; +} + +.status-chip { + padding: 10px 16px; + border-radius: 999px; + border: 1px solid var(--line); + background: var(--accent-soft); + color: var(--text); + font-weight: 600; +} + +.status-chip.good { + background: var(--good-soft); +} + +.status-chip.bad { + background: var(--bad-soft); +} + +.status-copy { + display: flex; + flex-direction: column; + align-items: flex-end; + color: var(--muted); + gap: 4px; +} + +.grid { + display: grid; + grid-template-columns: repeat(2, minmax(0, 1fr)); + gap: 18px; +} + +.card { + background: linear-gradient(180deg, rgba(17, 27, 32, 0.95), rgba(14, 22, 26, 0.95)); + border: 1px solid var(--line); + border-radius: 20px; + padding: 20px; + box-shadow: 0 16px 50px rgba(0, 0, 0, 0.22); +} + +.span-two { + grid-column: span 2; +} + +.metrics-grid { + display: grid; + grid-template-columns: repeat(4, minmax(0, 1fr)); + gap: 14px; +} + +.metric-tile, +.metric-line { + background: rgba(255, 255, 255, 0.02); + border: 1px solid var(--line); + border-radius: 14px; + padding: 16px; +} + +.metric-tile strong, +.metric-line strong { + display: block; + margin-top: 10px; + font-size: 1.7rem; +} + +.metric-tile small { + display: block; + margin-top: 8px; + color: var(--muted); + line-height: 1.5; +} + +.label, +.section-label { + color: var(--accent); + text-transform: uppercase; + letter-spacing: 0.12em; + font-size: 0.78rem; +} + +.card-header { + display: flex; + justify-content: space-between; + align-items: center; + gap: 12px; + margin-bottom: 16px; +} + +.card-header h2 { + margin: 0; + font-size: 1.2rem; +} + +.card-header span { + color: var(--muted); + font-size: 0.92rem; +} + +.three-col { + display: grid; + grid-template-columns: repeat(3, minmax(0, 1fr)); + gap: 16px; +} + +.data-list { + margin: 10px 0 0; + display: grid; + gap: 10px; +} + +.data-list div { + display: flex; + justify-content: space-between; + gap: 12px; + padding-bottom: 10px; + border-bottom: 1px solid var(--line); +} + +.data-list dt, +.data-list dd { + margin: 0; +} + +.data-list dt { + color: var(--muted); +} + +.pill-list { + display: flex; + flex-wrap: wrap; + gap: 10px; + margin-top: 12px; +} + +.pill { + padding: 8px 12px; + border-radius: 999px; + border: 1px solid var(--line); + background: rgba(255, 255, 255, 0.03); + font-size: 0.92rem; +} + +.pill.good { + background: var(--good-soft); +} + +.pill.bad { + background: var(--bad-soft); +} + +.table-wrap { + overflow-x: auto; +} + +table { + width: 100%; + border-collapse: collapse; +} + +th, +td { + text-align: left; + padding: 12px 10px; + border-bottom: 1px solid var(--line); + vertical-align: top; +} + +th { + color: var(--muted); + font-size: 0.82rem; + text-transform: uppercase; + letter-spacing: 0.12em; +} + +td { + line-height: 1.5; +} + +code { + font-family: "IBM Plex Mono", "SFMono-Regular", monospace; + font-size: 0.88rem; + color: #d4e8e3; +} + +@media (max-width: 1080px) { + .hero, + .card-header { + flex-direction: column; + align-items: flex-start; + } + + .grid, + .metrics-grid, + .three-col { + grid-template-columns: 1fr; + } + + .span-two { + grid-column: span 1; + } + + .status-panel { + align-items: flex-start; + } +} diff --git a/src/autonomy_metrics_webui/autonomy_metrics_webui/static/dashboard.js b/src/autonomy_metrics_webui/autonomy_metrics_webui/static/dashboard.js new file mode 100644 index 0000000..8c7d6f3 --- /dev/null +++ b/src/autonomy_metrics_webui/autonomy_metrics_webui/static/dashboard.js @@ -0,0 +1,163 @@ +function meters(value) { + return `${Number(value || 0).toFixed(2)} m`; +} + +function speed(value) { + return `${Number(value || 0).toFixed(2)} m/s`; +} + +function seconds(value) { + return `${Number(value || 0).toFixed(1)} s`; +} + +function setText(id, value) { + const element = document.getElementById(id); + if (element) { + element.textContent = value; + } +} + +function renderHeartbeats(heartbeats) { + const container = document.getElementById("heartbeatStates"); + container.innerHTML = ""; + Object.entries(heartbeats || {}).forEach(([name, healthy]) => { + const span = document.createElement("span"); + span.className = `pill ${healthy ? "good" : "bad"}`; + span.textContent = `${name}: ${healthy ? "OK" : "FAILED"}`; + container.appendChild(span); + }); + if (!Object.keys(heartbeats || {}).length) { + container.innerHTML = 'No heartbeat data yet'; + } +} + +function renderAreaTable(areaState) { + const body = document.getElementById("areaTableBody"); + body.innerHTML = ""; + Object.entries((areaState && areaState.states) || {}).forEach(([name, metrics]) => { + const row = document.createElement("tr"); + row.innerHTML = ` + ${name} + ${meters(metrics.distance_m)} + ${seconds(metrics.total_time_sec)} + ${seconds(metrics.average_time_per_visit_sec)} + ${speed(metrics.average_speed_mps)} + ${metrics.visits} + `; + body.appendChild(row); + }); + if (!body.children.length) { + body.innerHTML = 'No navigation area data yet'; + } +} + +function renderEvents(events) { + const body = document.getElementById("eventsTableBody"); + body.innerHTML = ""; + (events || []).forEach((event) => { + const row = document.createElement("tr"); + row.innerHTML = ` + ${event.time || "--"} + ${event.event_type || "--"} + ${JSON.stringify(event.details || {})} + `; + body.appendChild(row); + }); + if (!body.children.length) { + body.innerHTML = 'No events available'; + } +} + +function renderLive(data) { + const summary = data.summary || {}; + const mode = summary.mode || {}; + const motion = summary.motion || {}; + const interventions = summary.interventions || {}; + const robotState = summary.robot_state || {}; + const pathDeviation = summary.path_deviation || {}; + const navArea = summary.navigation_area || {}; + + const isAutonomous = Boolean(mode.autonomous); + const modeChip = document.getElementById("modeChip"); + modeChip.textContent = mode.label || "Waiting for summary"; + modeChip.className = `status-chip ${isAutonomous ? "good" : "bad"}`; + + setText("serverTimeLabel", data.server_time || "Server time unavailable"); + setText( + "heartbeatLabel", + data.heartbeat && data.heartbeat.seen + ? `Heartbeat age ${Number(data.heartbeat.age_sec || 0).toFixed(1)} s` + : "Heartbeat unknown" + ); + setText("summaryTimestamp", summary.timestamp || "No summary received yet"); + setText("mdbiValue", summary.mdbi_m == null ? "N/A" : meters(summary.mdbi_m)); + setText("interventionsValue", interventions.count ?? 0); + setText( + "activeDisabledValue", + robotState.active_to_disabled_in_autonomous_count ?? 0 + ); + setText("currentSpeedValue", speed(motion.current_speed_mps)); + setText("totalDistanceValue", meters(motion.total_distance_m)); + setText( + "autoDistanceValue", + meters(mode.distance_m ? mode.distance_m.Autonomous : 0) + ); + setText( + "manualDistanceValue", + meters(mode.distance_m ? mode.distance_m.Manual : 0) + ); + setText("autoTimeValue", seconds(mode.time_sec ? mode.time_sec.Autonomous : 0)); + setText("manualTimeValue", seconds(mode.time_sec ? mode.time_sec.Manual : 0)); + setText("avgSpeedValue", speed(motion.average_speed_mps)); + setText("areaCurrentValue", navArea.current || "Current area unknown"); + setText("pathCurrentValue", meters(pathDeviation.current_m)); + setText("pathAverageValue", meters(pathDeviation.average_m)); + setText("pathMaxValue", meters(pathDeviation.max_m)); + setText("pathSampleValue", `${pathDeviation.samples || 0} samples`); + + renderHeartbeats(summary.heartbeats || {}); + renderAreaTable(navArea); +} + +function renderHistory(data) { + if (data.session) { + setText( + "sessionInfo", + `Session ${data.session._id} started ${data.session.session_start_time}` + ); + } + renderEvents(data.events || []); +} + +async function loadLive() { + const response = await fetch("/api/live"); + const data = await response.json(); + renderLive(data); +} + +async function loadHistory() { + const response = await fetch("/api/history?events=20&snapshots=10"); + const data = await response.json(); + renderHistory(data); +} + +async function refresh() { + try { + await loadLive(); + } catch (error) { + setText("serverTimeLabel", `Live refresh failed: ${error.message}`); + } +} + +async function refreshHistory() { + try { + await loadHistory(); + } catch (error) { + setText("sessionInfo", `History refresh failed: ${error.message}`); + } +} + +refresh(); +refreshHistory(); +setInterval(refresh, 1000); +setInterval(refreshHistory, 10000); diff --git a/src/autonomy_metrics_webui/autonomy_metrics_webui/templates/index.html b/src/autonomy_metrics_webui/autonomy_metrics_webui/templates/index.html new file mode 100644 index 0000000..9889666 --- /dev/null +++ b/src/autonomy_metrics_webui/autonomy_metrics_webui/templates/index.html @@ -0,0 +1,147 @@ + + + + + + Autonomy Metrics Dashboard + + + +
+
+
+

Autonomy Metrics

+

Operational Robustness Dashboard

+

+ Live autonomy state, intervention quality, motion coverage, path deviation, + and MongoDB-backed session history. +

+
+
+
Waiting for summary
+
+ Heartbeat unknown + Server time +
+
+
+ +
+
+
+ MDBI + -- + Mean distance between interventions +
+
+ Interventions + 0 + Autonomous to manual with failing heartbeat +
+
+ Active to Disabled + 0 + Only counted while autonomous +
+
+ Current Speed + 0.00 m/s + Odometry-derived live speed +
+
+ +
+
+

Mode Summary

+ No summary received yet +
+
+
+

Distance

+
+
Total
0.00 m
+
Autonomous
0.00 m
+
Manual
0.00 m
+
+
+
+

Time

+
+
Autonomous
0.0 s
+
Manual
0.0 s
+
Average Speed
0.00 m/s
+
+
+
+

Heartbeat Gates

+
+
+
+
+ +
+
+

Navigation Area Metrics

+ Current area unknown +
+
+ + + + + + + + + + + + +
AreaDistanceTotal TimeAvg Time / VisitAvg SpeedVisits
+
+
+ +
+
+

Path Deviation

+ 0 samples +
+
+
+ Current + 0.00 m +
+
+ Average + 0.00 m +
+
+ Maximum + 0.00 m +
+
+
+ +
+
+

Recent Events

+ No session loaded +
+
+ + + + + + + + + +
TimeEventDetails
+
+
+
+
+ + + diff --git a/src/autonomy_metrics_webui/autonomy_metrics_webui/webui_server.py b/src/autonomy_metrics_webui/autonomy_metrics_webui/webui_server.py new file mode 100644 index 0000000..7c4538e --- /dev/null +++ b/src/autonomy_metrics_webui/autonomy_metrics_webui/webui_server.py @@ -0,0 +1,265 @@ +#!/usr/bin/env python3 +"""ROS 2 + Flask backend for the autonomy metrics dashboard.""" + +from __future__ import annotations + +from datetime import datetime, timezone +import json +import logging +from pathlib import Path +import threading + +from flask import Flask, jsonify, render_template, request +from werkzeug.serving import make_server + +import rclpy +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node + +from ament_index_python.packages import get_package_share_directory +from std_msgs.msg import Bool, String + +from autonomy_metrics.db_mgr import DatabaseMgr + + +class _ServerThread(threading.Thread): + def __init__(self, host: str, port: int, app: Flask): + super().__init__(daemon=True) + self._server = make_server(host, port, app, threaded=True) + + def run(self): + self._server.serve_forever() + + def shutdown(self): + self._server.shutdown() + + +class AutonomyMetricsWebUiServer(Node): + """Serve a live dashboard backed by ROS summary topics and MongoDB history.""" + + def __init__(self): + super().__init__("autonomy_metrics_webui_server") + + self.declare_parameter("ui_host", "0.0.0.0") + self.declare_parameter("ui_port", 8081) + self.declare_parameter("ui_access_log", False) + self.declare_parameter("mongodb_host", "localhost") + self.declare_parameter("mongodb_port", 27018) + self.declare_parameter("remote_mongodb_host", "") + self.declare_parameter("remote_mongodb_port", 27017) + self.declare_parameter("prefer_remote_data", False) + self.declare_parameter("database_name", "robot_incidents") + self.declare_parameter("session_summary_topic", "/autonomy_metrics/session_summary_json") + self.declare_parameter("heartbeat_topic", "/autonomy_metrics/heartbeat") + + self._host = self.get_parameter("ui_host").value + self._port = int(self.get_parameter("ui_port").value) + self._ui_access_log = bool(self.get_parameter("ui_access_log").value) + self._summary_topic = self.get_parameter("session_summary_topic").value + self._heartbeat_topic = self.get_parameter("heartbeat_topic").value + self._database_name = self.get_parameter("database_name").value + + self._latest_summary = {} + self._summary_received_at = None + self._heartbeat_seen = False + self._heartbeat_received_at = None + self._lock = threading.Lock() + + self._db_reader = self._create_db_reader( + prefer_remote=bool(self.get_parameter("prefer_remote_data").value) + ) + + self.create_subscription(String, self._summary_topic, self._summary_callback, 10) + self.create_subscription(Bool, self._heartbeat_topic, self._heartbeat_callback, 10) + + template_folder = ( + Path(get_package_share_directory("autonomy_metrics_webui")) / "templates" + ) + static_folder = Path(get_package_share_directory("autonomy_metrics_webui")) / "static" + self._app = Flask( + __name__, + template_folder=str(template_folder), + static_folder=str(static_folder), + ) + self._configure_routes() + + if not self._ui_access_log: + logging.getLogger("werkzeug").setLevel(logging.ERROR) + + self._server_thread = _ServerThread(self._host, self._port, self._app) + self._server_thread.start() + self.get_logger().info( + f"Autonomy metrics dashboard available at http://{self._host}:{self._port}" + ) + + def _create_db_reader(self, prefer_remote: bool): + ordered_targets = [] + if prefer_remote: + ordered_targets.extend( + [ + ( + self.get_parameter("remote_mongodb_host").value, + int(self.get_parameter("remote_mongodb_port").value), + ), + ( + self.get_parameter("mongodb_host").value, + int(self.get_parameter("mongodb_port").value), + ), + ] + ) + else: + ordered_targets.extend( + [ + ( + self.get_parameter("mongodb_host").value, + int(self.get_parameter("mongodb_port").value), + ), + ( + self.get_parameter("remote_mongodb_host").value, + int(self.get_parameter("remote_mongodb_port").value), + ), + ] + ) + + for host, port in ordered_targets: + if not host: + continue + try: + reader = DatabaseMgr( + database_name=self._database_name, + host=host, + port=port, + ) + reader.client.admin.command("ping") + self.get_logger().info(f"Web UI using MongoDB reader {host}:{port}") + return reader + except Exception as exc: + self.get_logger().warn(f"MongoDB reader unavailable at {host}:{port}: {exc}") + return None + + def _configure_routes(self): + @self._app.get("/") + def index(): + return render_template("index.html") + + @self._app.get("/api/health") + def api_health(): + return jsonify( + { + "ok": True, + "server_time": self._utc_now_iso(), + "summary_seen": self._summary_received_at is not None, + "heartbeat_seen": self._heartbeat_seen, + } + ) + + @self._app.get("/api/live") + def api_live(): + with self._lock: + summary = dict(self._latest_summary) + summary_received_at = self._summary_received_at + heartbeat_seen = self._heartbeat_seen + heartbeat_received_at = self._heartbeat_received_at + + if not summary and self._db_reader is not None: + latest_session = self._db_reader.fetch_latest_session() + if latest_session: + summary = latest_session.get("summary", {}) + + return jsonify( + { + "server_time": self._utc_now_iso(), + "summary_received_at": summary_received_at, + "summary": summary, + "heartbeat": { + "seen": heartbeat_seen, + "received_at": heartbeat_received_at, + "age_sec": self._age_seconds(heartbeat_received_at), + }, + } + ) + + @self._app.get("/api/history") + def api_history(): + if self._db_reader is None: + return jsonify( + { + "session": None, + "events": [], + "snapshots": [], + "error": "MongoDB reader is unavailable", + } + ) + + event_limit = max(1, min(100, int(request.args.get("events", 25)))) + snapshot_limit = max(1, min(100, int(request.args.get("snapshots", 10)))) + + latest_session = self._db_reader.fetch_latest_session() + session_id = latest_session["_id"] if latest_session else None + + events = self._db_reader.fetch_recent_events( + limit=event_limit, + session_id=session_id, + ) + snapshots = self._db_reader.fetch_recent_snapshots( + limit=snapshot_limit, + session_id=session_id, + ) + return jsonify( + { + "session": latest_session, + "events": events, + "snapshots": snapshots, + "error": None, + } + ) + + def _summary_callback(self, msg: String): + try: + summary = json.loads(msg.data) + except json.JSONDecodeError as exc: + self.get_logger().warn(f"Failed to parse summary JSON: {exc}") + return + + with self._lock: + self._latest_summary = summary + self._summary_received_at = self._utc_now_iso() + + def _heartbeat_callback(self, msg: Bool): + with self._lock: + self._heartbeat_seen = bool(msg.data) + self._heartbeat_received_at = self._utc_now_iso() + + def destroy_node(self): + if hasattr(self, "_server_thread"): + self._server_thread.shutdown() + self._server_thread.join(timeout=2.0) + return super().destroy_node() + + def _age_seconds(self, timestamp: str | None): + if not timestamp: + return None + try: + seen_at = datetime.fromisoformat(timestamp) + except ValueError: + return None + return max( + 0.0, + (datetime.now(tz=timezone.utc) - seen_at.astimezone(timezone.utc)).total_seconds(), + ) + + def _utc_now_iso(self): + return datetime.now(tz=timezone.utc).isoformat() + + +def main(args=None): + rclpy.init(args=args) + node = AutonomyMetricsWebUiServer() + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() diff --git a/src/autonomy_metrics_webui/launch/autonomy_metrics_webui.launch.py b/src/autonomy_metrics_webui/launch/autonomy_metrics_webui.launch.py new file mode 100644 index 0000000..75a4aaa --- /dev/null +++ b/src/autonomy_metrics_webui/launch/autonomy_metrics_webui.launch.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription( + [ + DeclareLaunchArgument("ui_host", default_value="0.0.0.0"), + DeclareLaunchArgument("ui_port", default_value="8081"), + DeclareLaunchArgument("ui_access_log", default_value="false"), + DeclareLaunchArgument("mongodb_host", default_value="localhost"), + DeclareLaunchArgument("mongodb_port", default_value="27018"), + DeclareLaunchArgument("remote_mongodb_host", default_value=""), + DeclareLaunchArgument("remote_mongodb_port", default_value="27017"), + DeclareLaunchArgument("prefer_remote_data", default_value="false"), + DeclareLaunchArgument("database_name", default_value="robot_incidents"), + Node( + package="autonomy_metrics_webui", + executable="autonomy_metrics_webui_server", + name="autonomy_metrics_webui_server", + output="screen", + parameters=[ + { + "ui_host": LaunchConfiguration("ui_host"), + "ui_port": LaunchConfiguration("ui_port"), + "ui_access_log": LaunchConfiguration("ui_access_log"), + "mongodb_host": LaunchConfiguration("mongodb_host"), + "mongodb_port": LaunchConfiguration("mongodb_port"), + "remote_mongodb_host": LaunchConfiguration("remote_mongodb_host"), + "remote_mongodb_port": LaunchConfiguration("remote_mongodb_port"), + "prefer_remote_data": LaunchConfiguration("prefer_remote_data"), + "database_name": LaunchConfiguration("database_name"), + } + ], + ), + ] + ) diff --git a/src/autonomy_metrics_webui/package.xml b/src/autonomy_metrics_webui/package.xml new file mode 100644 index 0000000..2db5ca1 --- /dev/null +++ b/src/autonomy_metrics_webui/package.xml @@ -0,0 +1,22 @@ + + + + autonomy_metrics_webui + 0.1.0 + Web UI package for live autonomy metrics, intervention history, and robustness monitoring. + + Ibrahim + Apache 2.0 + + rclpy + std_msgs + autonomy_metrics + python3-flask + python3-pymongo + launch + launch_ros + + + ament_python + + diff --git a/src/autonomy_metrics_webui/resource/autonomy_metrics_webui b/src/autonomy_metrics_webui/resource/autonomy_metrics_webui new file mode 100644 index 0000000..559120c --- /dev/null +++ b/src/autonomy_metrics_webui/resource/autonomy_metrics_webui @@ -0,0 +1 @@ +autonomy_metrics_webui diff --git a/src/autonomy_metrics_webui/setup.cfg b/src/autonomy_metrics_webui/setup.cfg new file mode 100644 index 0000000..3ee302a --- /dev/null +++ b/src/autonomy_metrics_webui/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/autonomy_metrics_webui + +[install] +install_scripts=$base/lib/autonomy_metrics_webui diff --git a/src/autonomy_metrics_webui/setup.py b/src/autonomy_metrics_webui/setup.py new file mode 100644 index 0000000..6aa0ffb --- /dev/null +++ b/src/autonomy_metrics_webui/setup.py @@ -0,0 +1,30 @@ +from glob import glob +from setuptools import setup + + +package_name = "autonomy_metrics_webui" + + +setup( + name=package_name, + version="0.1.0", + packages=[package_name], + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ("share/" + package_name + "/launch", glob("launch/*.py")), + ("share/" + package_name + "/templates", glob("autonomy_metrics_webui/templates/*")), + ("share/" + package_name + "/static", glob("autonomy_metrics_webui/static/*")), + ], + install_requires=["setuptools", "flask"], + zip_safe=True, + maintainer="Ibrahim", + maintainer_email="ibrahim.hroub7@gmail.com", + description="Industrial-style web UI for autonomy metrics and robustness monitoring.", + license="Apache 2.0", + entry_points={ + "console_scripts": [ + "autonomy_metrics_webui_server = autonomy_metrics_webui.webui_server:main", + ], + }, +) diff --git a/tools/fake_autonomy_metrics_stream.py b/tools/fake_autonomy_metrics_stream.py new file mode 100644 index 0000000..4ebdbaf --- /dev/null +++ b/tools/fake_autonomy_metrics_stream.py @@ -0,0 +1,188 @@ +#!/usr/bin/env python3 +"""Deterministic fake ROS topic stream for autonomy_metrics integration testing.""" + +from __future__ import annotations + +import rclpy +from rclpy.node import Node + +from builtin_interfaces.msg import Time +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Odometry, Path +from std_msgs.msg import Bool, String + + +class FakeAutonomyMetricsStream(Node): + def __init__(self): + super().__init__("fake_autonomy_metrics_stream") + + self.autonomous_mode_pub = self.create_publisher(Bool, "/autonomous_mode", 10) + self.autonomy_checks_pub = self.create_publisher( + Bool, "/autonomy_checks/heartbeat", 10 + ) + self.navigation_safe_pub = self.create_publisher( + Bool, "/navigation_safe/heartbeat", 10 + ) + self.system_check_pub = self.create_publisher(Bool, "/system_check/heartbeat", 10) + self.robot_state_pub = self.create_publisher(String, "/robot_state", 10) + self.odom_pub = self.create_publisher( + Odometry, "/gophar_vehicle_controller/odometry", 10 + ) + self.area_pub = self.create_publisher(String, "/robot_navigation_area", 10) + self.path_pub = self.create_publisher(Path, "/plan", 10) + self.global_odom_pub = self.create_publisher(Odometry, "/odometry/global", 10) + + self.start_time = self.get_clock().now() + self.tick_count = 0 + self.done = False + self.timer = self.create_timer(0.2, self._tick) + + self._publish_path() + + def _tick(self): + elapsed = (self.get_clock().now() - self.start_time).nanoseconds * 1e-9 + self.tick_count += 1 + + autonomous_mode = False + autonomy_checks = True + navigation_safe = True + system_check = True + robot_state = "Disabled" + area = "OUTSIDE_POLYTUNNEL" + odom_x = 0.0 + odom_y = 0.0 + global_y = 0.05 + speed = 0.0 + + if elapsed < 1.0: + pass + elif elapsed < 4.0: + autonomous_mode = True + robot_state = "Active" + area = "OUTSIDE_POLYTUNNEL" + odom_x = 0.8 * (elapsed - 1.0) + speed = 0.8 + global_y = 0.10 + elif elapsed < 6.0: + autonomous_mode = True + robot_state = "Active" + area = "TRANSITION_INTO_POLYTUNNEL" + odom_x = 2.4 + 0.6 * (elapsed - 4.0) + speed = 0.6 + global_y = 0.25 + elif elapsed < 7.0: + autonomous_mode = True + robot_state = "Disabled" + area = "INSIDE_POLYTUNNEL" + odom_x = 3.6 + 0.2 * (elapsed - 6.0) + speed = 0.2 + global_y = 0.35 + elif elapsed < 8.0: + autonomous_mode = True + system_check = False + robot_state = "Active" + area = "INSIDE_POLYTUNNEL" + odom_x = 3.8 + 0.4 * (elapsed - 7.0) + speed = 0.4 + global_y = 0.45 + elif elapsed < 9.5: + autonomous_mode = False + system_check = False + robot_state = "Enabled" + area = "INSIDE_POLYTUNNEL" + odom_x = 4.2 + 0.3 * (elapsed - 8.0) + speed = 0.3 + global_y = 0.40 + elif elapsed < 12.0: + autonomous_mode = True + robot_state = "Active" + area = "INSIDE_POLYTUNNEL" + odom_x = 4.65 + 0.9 * (elapsed - 9.5) + speed = 0.9 + global_y = 0.20 + elif elapsed < 13.5: + autonomous_mode = True + autonomy_checks = False + robot_state = "Active" + area = "OUTSIDE_POLYTUNNEL" + odom_x = 6.9 + 0.5 * (elapsed - 12.0) + speed = 0.5 + global_y = 0.60 + elif elapsed < 15.0: + autonomous_mode = False + autonomy_checks = False + robot_state = "Enabled" + area = "OUTSIDE_POLYTUNNEL" + odom_x = 7.65 + 0.2 * (elapsed - 13.5) + speed = 0.2 + global_y = 0.55 + else: + self.get_logger().info("Fake data scenario complete.") + self.timer.cancel() + self.done = True + return + + self._publish_bool(self.autonomous_mode_pub, autonomous_mode) + self._publish_bool(self.autonomy_checks_pub, autonomy_checks) + self._publish_bool(self.navigation_safe_pub, navigation_safe) + self._publish_bool(self.system_check_pub, system_check) + self._publish_string(self.robot_state_pub, robot_state) + self._publish_string(self.area_pub, area) + self._publish_odometry(self.odom_pub, odom_x, odom_y, speed) + self._publish_odometry(self.global_odom_pub, odom_x, global_y, speed) + + def _publish_path(self): + path = Path() + path.header.frame_id = "map" + path.header.stamp = self._stamp() + for index in range(0, 21): + pose = PoseStamped() + pose.header.frame_id = "map" + pose.header.stamp = self._stamp() + pose.pose.position.x = float(index) * 0.5 + pose.pose.position.y = 0.0 + pose.pose.orientation.w = 1.0 + path.poses.append(pose) + self.path_pub.publish(path) + + def _publish_odometry(self, publisher, x: float, y: float, speed: float): + odom = Odometry() + odom.header.frame_id = "map" + odom.header.stamp = self._stamp() + odom.pose.pose.position.x = float(x) + odom.pose.pose.position.y = float(y) + odom.pose.pose.orientation.w = 1.0 + odom.twist.twist.linear.x = float(speed) + publisher.publish(odom) + + def _publish_bool(self, publisher, value: bool): + msg = Bool() + msg.data = bool(value) + publisher.publish(msg) + + def _publish_string(self, publisher, value: str): + msg = String() + msg.data = value + publisher.publish(msg) + + def _stamp(self) -> Time: + now = self.get_clock().now().to_msg() + return Time(sec=now.sec, nanosec=now.nanosec) + + +def main(): + rclpy.init() + node = FakeAutonomyMetricsStream() + try: + while rclpy.ok() and not node.done: + rclpy.spin_once(node, timeout_sec=0.5) + except KeyboardInterrupt: + pass + finally: + if node.context.ok(): + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()