diff --git a/README.md b/README.md index a98add0..c62fcc8 100644 --- a/README.md +++ b/README.md @@ -1,67 +1,521 @@ -# Autonomy Metrics Logger Node +# Autonomy Metrics Logger -The Autonomy Metrics Logger is a ROS2 node that tracks various robot events and computes the **Mean Distance Between Incidents (MDBI)**. This node monitors topics such as GPS, odometry, battery status, and emergency stop status to log incidents and key metrics into a MongoDB database. +`autonomy_metrics_logger` is a ROS 2 node that: -## Features +- 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). -- **MDBI Calculation**: Computes the Mean Distance Between Incidents based on traveled distance and logged incidents. -- **Autonomous Mode Tracking**: Records the time spent and distance covered in autonomous mode. -- **Event Logging**: Logs critical events like emergency stops, manual overrides, and coordinator tasks. -- **Database Integration**: Uses MongoDB to store event logs, distances, and operational details of the robot. -- **Heartbeat Signal**: Publishes a heartbeat message every 5 seconds to indicate the node is alive. -- **Git Repositories Logging**: Captures and stores Git information of the active repositories for traceability. +--- -## Usage +## 1. High-Level Behaviour -1. **Set Environment Variables:** +The node: - Configure the robot session by setting the following environment variables: +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. - ```sh - export ROBOT_NAME= - export FARM_NAME= - export FIELD_NAME= - export APPLICATION= - export SCENARIO_NAME= - ``` +MDBI is computed as: -2. **Run the Node:** +> `mdbi = autonomous_distance / incidents` +> (If `incidents == 0`, `mdbi = autonomous_distance`.) - ```sh - ros2 launch autonomy_metrics autonomy_metrics.launch.py - ``` +--- -## Parameters +## 2. MongoDB Integration -The node uses the following ROS parameters, which can be customized: +The node creates a document in `robot_incidents.sessions` per run: -- `mongodb_host` (default: `localhost`): MongoDB server host. -- `mongodb_port` (default: `27017`): MongoDB server port. -- `aoc_scenario_path` (default: `""`): Path to the AOC scenario repository. -- `aoc_navigation_path` (default: `""`): Path to the AOC navigation repository. -- `min_distance_threshold` (default: `0.2`): Minimum distance threshold for recording traveled distance (in meters). -- `gps_topic` (default: `/gps_base/fix`) -- `odometry_topic` (default: `/diff_drive_controller/odom`) -- `battery_status_topic` (default: `/diff_drive_controller/battery_status`) -- `estop_status_topic` (default: `/diff_drive_controller/estop_status`) -- `hunter_status_topic` (default: `/hunter_status`) -- `actioned_by_coordinator_topic` (default: `/topological_navigation/execute_policy_mode/goal`) +```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 }, + "...": "..." + } + } + } + ] +} +```` -## Node Details +Environment variables used to tag the session: -### Subscriptions +* `ROBOT_NAME` +* `FARM_NAME` +* `FIELD_NAME` +* `APPLICATION` +* `SCENARIO_NAME` -- **GPS Data** (`/gps_base/fix`): Receives and logs GPS data of the robot. -- **Odometry Data** (`/diff_drive_controller/odom`): Monitors odometry data to calculate traveled distance. -- **Battery Status** (`/diff_drive_controller/battery_status`): Logs battery level information. -- **Emergency Stop Status** (`/diff_drive_controller/estop_status`): Monitors and logs emergency stop events. -- **Hunter Status** (`/hunter_status`): Tracks the robot's current status, including operation mode and battery voltage. -- **Coordinator Task** (`/topological_navigation/execute_policy_mode/goal`): Logs tasks received from the coordinator. +--- -### Publications +## 3. Node Parameters -- **Heartbeat** (`mdbi_logger/heartbeat`): Publishes a signal every 5 seconds to indicate node activity. +All parameters are exposed through the launch file. -## License +### Core configuration -Licensed under the Apache License, Version 2.0. See the [LICENSE](http://www.apache.org/licenses/LICENSE-2.0) file for more details. +| 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 + +```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`. + +* `battery_field` (optional, string) + Field name under this topic that carries battery percentage; updates `current_battery`. + +#### Dynamic republishing + +```yaml +publish: + enable: true + topic: "mdbi_logger/battery_level" + type: "std_msgs/msg/Float32" + field: "battery_percentage" +``` + +* `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`: + + * `EMS` intervention is logged. + * Incidents increment only if mode = Autonomous. + +#### YAML-driven interventions + +Per-topic: + +* `intervention_on_message`: + + ```yaml + intervention_on_message: + enable: true + event_type: "Joy_override" + ``` + + If enabled, **every message** on that topic triggers an event of this type via `trigger_intervention`. + +* `intervention_on_change`: + + ```yaml + intervention_on_change: + fault_shutdown: + trigger_value: true + event_type: "Fault_shutdown" + ``` + + * 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. + +--- + +## 5. Mode Handling and Incidents + +* 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, + ]) +``` + +### Example launch usage + +Minimal: + +```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 +``` + +With remote DB + tuned thresholds: + +```bash +ros2 launch autonomy_metrics_logger autonomy_metrics_logger.launch.py \ + config_yaml:=/path/to/metrics_gophar.yaml \ + 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 +``` + +--- + +## 7. Testing Tips + +* Verify topics: + + ```bash + ros2 topic list + ros2 topic info /gophar_vehicle_controller/odometry + ros2 topic info /gophar/system_status + ``` + +* Trigger joystick interventions: + + ```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}}" + ``` + +* Check DB content (Mongo shell or Mongo client) in `robot_incidents.sessions`. + +The README covers: + +* All node parameters, +* YAML configuration options, +* How incidents, MDBI, collisions, and snapshots are computed, +* How to launch and test the system. diff --git a/src/autonomy_metrics/autonomy_metrics/db_mgr.py b/src/autonomy_metrics/autonomy_metrics/db_mgr.py index 1651799..00355f9 100644 --- a/src/autonomy_metrics/autonomy_metrics/db_mgr.py +++ b/src/autonomy_metrics/autonomy_metrics/db_mgr.py @@ -72,6 +72,7 @@ def init_session(self, env_variables, aoc_repos_info): "incidents": 0, "distance": 0, "autonomous_distance": 0, + "collision_incidents": 0, # NEW "events": [] } result = self.sessions_collection.insert_one(session_document) @@ -155,4 +156,20 @@ def update_mdbi(self, mdbi): {"_id": ObjectId(self.session_id)}, {"$set": {"mdbi": mdbi}} ) - return result.modified_count > 0 \ No newline at end of file + return result.modified_count > 0 + + def update_collision_incidents(self, collision_incidents): + """ + Updates the collision_incidents value of the current session. + + Args: + collision_incidents (int): The new collision incidents value. + + Returns: + bool: True if the value was updated successfully, False otherwise. + """ + result = self.sessions_collection.update_one( + {"_id": ObjectId(self.session_id)}, + {"$set": {"collision_incidents": collision_incidents}} + ) + return result.modified_count > 0 diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index 098a480..d6573a6 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -1,99 +1,227 @@ - +#!/usr/bin/env python3 """ -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. - -Author: Ibrahim Hroob -Email: ibrahim.hroub7@gmail.com +YAML-driven AutonomyMetricsLogger +Author: Ibrahim Hroob - JABASAI """ import os import math -import rclpy import subprocess -from rclpy.node import Node -from sensor_msgs.msg import NavSatFix -from std_msgs.msg import Float32, Bool, Int8, String -from nav_msgs.msg import Odometry -from topological_navigation_msgs.msg import ExecutePolicyModeGoal -from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr +import yaml +import time from datetime import datetime, timezone +from importlib import import_module + +import rclpy +from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data +from rclpy.time import Time -# Try to import HunterStatus and handle if it is unavailable -try: - from hunter_msgs.msg import HunterStatus - HUNTER_MSG_AVAILABLE = True -except ImportError: - HUNTER_MSG_AVAILABLE = False - print("Warning: hunter_msgs.HunterStatus is not available. Hunter status callbacks will be disabled.") - -# Try to import DogroothStatus and handle if it is unavailable -try: - from dogtooth_msgs.msg import DogtoothStatus - DOGTOOTH_MSG_AVAILABLE = True -except ImportError: - DOGTOOTH_MSG_AVAILABLE = False - print("Warning: hunter_msgs.HunterStatus is not available. Hunter status callbacks will be disabled.") +# Keep a few explicit imports used by default publishers (std msgs) +from std_msgs.msg import Bool, Float32, Int32 -class AutonomyMetricsLogger(Node): +from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr + + +def ros_msg_to_dict(msg): """ - A class for logging robot events to calculate MDBI (Mean Distance Between Incidents). - - Attributes: - mdbi (float): Mean Distance Between Incidents. - distance (float): Traveled distance in meters. - db_mgr (DBMgr): Instance of DatabaseMgr for database operations. - gps_data (dict): GPS data of the robot. - estop (bool): Emergency stop status. - battery_level (float): Battery level of the robot. - operation_mode (str): Operation mode of the robot. - previous_x (float): Previous x-coordinate for distance calculation. - previous_y (float): Previous y-coordinate for distance calculation. - env_variables (dict): Environmental variables for the robot session. + 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'") + + 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}") + +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}") + + +class AutonomyMetricsLogger(Node): def __init__(self): - super().__init__('mdbi_logger') - self.get_logger().info('Initializing logging node') - - # Define useful global variables - self.mongo_host = None - self.mongo_port = None - self.aoc_scenario_path = '' - self.aoc_navigation_path = '' - self.mdbi = None # Mean Distance Between Incidents + 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}" + ) + + # 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.first_gps_fix_received = False - self.tasks_received_from_coordinator = 0 - self.distance = 0 # Total traveled distance in meters - self.autonomous_distance = 0 - self.autonomous_time = 0 # Total time spent in autonomous mode (in seconds) - self.autonomous_start_time = None # Time when autonomous mode started - self.min_distance_threshold = 0.2 #meters - self.AUTO = 'Autonomous' - self.MAN = 'Manual' - self.prev_speed = 0.0 self.speed = 0.0 + self.last_odom_update_time = self.get_clock().now() + + self.dynamic_subs = [] + self.dynamic_publishers = {} + 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) - # Init some variables - self.details = {} - self.details['estop'] = False - self.details['operation_mode'] = self.MAN + self.heartbeat_timer = self.create_timer(1.0, self.timer_callback) - # Get robot name and other environmental variables (Group them into a dictionary) + self.load_and_setup_config() + + # 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)" + ) + + # Init Session env_variables = { 'robot_name': os.getenv('ROBOT_NAME', 'UNDEFINED'), 'farm_name': os.getenv('FARM_NAME', 'UNDEFINED'), @@ -101,428 +229,877 @@ def __init__(self): 'application': os.getenv('APPLICATION', 'UNDEFINED'), 'scenario_name': os.getenv('SCENARIO_NAME', 'UNDEFINED'), } - - # Declare and get the ROS parameters, including MongoDB host and port - self.declare_and_get_parameters() - # Initialize DatabaseMgr with the retrieved port - self.db_mgr = DBMgr(host=self.mongo_host, port=self.mongo_port) + 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']}" + ) - # Get aoc git repos info - try: - self.get_logger().info("Retrieving AOC git repository information...") - aoc_scenario_git_info = self.get_git_info(self.aoc_scenario_path) - except Exception as e: - self.get_logger().error(f"Error while logging git info: {e}") - aoc_scenario_git_info = None + 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: - self.get_logger().info("Retrieving AOC Navigation git repository information...") - aoc_navigation_git_info = self.get_git_info(self.aoc_navigation_path) + 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"Error while logging git info: {e}") - aoc_navigation_git_info = None + self.get_logger().error(f"Failed to load YAML: {e}") + self.config = {} + return - git_repos = [] - git_repos.append({'aoc_scenario_git_info': aoc_scenario_git_info}) - git_repos.append({'aoc_navigation_git_info': aoc_navigation_git_info}) + 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 + ) + 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 + + self.get_logger().info( + f"[Intervention] type={event_type}, mode={current_mode}, " + f"counted={count_incident}, total_incidents={self.incidents}" + ) + + 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}'" + ) + 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" + ) + 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.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 - # Subscribe to the topics - self.create_subscriptions() + current_time = self.get_clock().now() - # Initialize session in the database - self.db_mgr.init_session(env_variables, git_repos) + 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 - # Heartbeat publisher - self.heartbeat_publisher = self.create_publisher(Bool, 'mdbi_logger/heartbeat', 10) - self.heartbeat_timer = self.create_timer(1.0, self.publish_heartbeat) # Publish every 1 seconds + dx = pos.x - self.previous_x + dy = pos.y - self.previous_y + dist = math.sqrt(dx * dx + dy * dy) - # Add publishers for distance, incidents, and speed - self.distance_publisher = self.create_publisher(Float32, 'mdbi_logger/total_traveled_distance', 10) - self.incidents_publisher = self.create_publisher(Int8, 'mdbi_logger/total_incidents', 10) - self.speed_publisher = self.create_publisher(Float32, 'mdbi_logger/robot_speed', 10) - self.battery_publisher = self.create_publisher(Float32, 'mdbi_logger/battery_level', 10) + if dist < self.min_distance_threshold: + # Too small to count as movement + return - def publish_heartbeat(self): - # Create and publish the heartbeat message - heartbeat_msg = Bool() - heartbeat_msg.data = True # Set to True to indicate the node is alive - self.heartbeat_publisher.publish(heartbeat_msg) - self.get_logger().debug('Heartbeat published') + if not hasattr(self, 'previous_time'): + self.previous_time = current_time - if self.prev_speed != self.speed: - self.prev_speed = self.speed - else: - self.publish_speed(0.0) + time_diff = (current_time - self.previous_time).nanoseconds * 1e-9 - def declare_and_get_parameters(self): - # Declare MongoDB host and port as ROS parameters - self.declare_parameter('mongodb_host', 'localhost') - self.declare_parameter('mongodb_port', 27017) - # Retrieve the parameters - 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 - - # Log MongoDB host and port - self.get_logger().info(f"MongoDB Host: {self.mongo_host}") - self.get_logger().info(f"MongoDB Port: {self.mongo_port}") + self.speed = dist / time_diff if time_diff > 0 else 0.0 + self.distance += dist - # Declare aoc repos path params - self.declare_parameter('aoc_scenario_path', '') - self.declare_parameter('aoc_navigation_path', '') - # Retrieve the parameters - self.aoc_scenario_path = self.get_parameter('aoc_scenario_path').get_parameter_value().string_value - self.aoc_navigation_path = self.get_parameter('aoc_navigation_path').get_parameter_value().string_value + # 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) - # Log aoc_scenario_path and aoc_navigation_path - self.get_logger().info(f"AOC Scenario Path: {self.aoc_scenario_path}") - self.get_logger().info(f"AOC Navigation Path: {self.aoc_navigation_path}") + 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 - self.declare_parameter('min_distance_threshold', 0.2) - self.min_distance_threshold = self.get_parameter('min_distance_threshold').get_parameter_value().double_value - self.get_logger().info(f"min_distance_threshold: {self.min_distance_threshold}") - - param_defaults = { - 'gps_topic': '/gps_base/fix', - 'odometry_topic': '/diff_drive_controller/odom', - 'battery_status_topic': '/diff_drive_controller/battery_status', - 'estop_status_topic': '/diff_drive_controller/estop_status', - 'hunter_status_topic': '/hunter_status', # Change this line - 'dogtooth_status_topic': '/dogtooth_status', # Change this line - 'actioned_by_coordinator_topic': '/topological_navigation/execute_policy_mode/goal' + # Keep metrics in the snapshot too + self.system_snapshot['metrics'] = { + 'distance': self.distance, + 'autonomous_distance': self.autonomous_distance, + 'speed': self.speed, } - self.params = {} - for param, default in param_defaults.items(): - self.declare_parameter(param, default) - self.params[param] = self.get_parameter(param).get_parameter_value().string_value - - for key, value in self.params.items(): - self.get_logger().info(f"{key.replace('_', ' ').title()}: {value}") - - def create_subscriptions(self): - self.create_subscription(Float32, self.params['battery_status_topic'], self.battery_level_callback, qos_profile=qos_profile_sensor_data) - self.create_subscription(Bool, self.params['estop_status_topic'], self.estop_sub_callback, qos_profile=qos_profile_sensor_data) - self.create_subscription(NavSatFix, self.params['gps_topic'], self.gps_fix_callback, qos_profile=qos_profile_sensor_data) - self.create_subscription(Odometry, self.params['odometry_topic'], self.gps_odom_callback, qos_profile=qos_profile_sensor_data) - self.create_subscription(ExecutePolicyModeGoal, self.params['actioned_by_coordinator_topic'], self.coordinator_callback, qos_profile=qos_profile_sensor_data) - # Only subscribe to hunter status if the message is available - if HUNTER_MSG_AVAILABLE: - self.create_subscription(HunterStatus, self.params['hunter_status_topic'], self.hunter_status_callback, qos_profile=qos_profile_sensor_data) - else: - self.get_logger().warn("HunterStatus message not available, skipping hunter status subscription.") - # Only subscribe to dogtooth status if the message is available - if DOGTOOTH_MSG_AVAILABLE: - self.create_subscription(DogtoothStatus, self.params['dogtooth_status_topic'], self.dogtooth_status_callback, qos_profile=qos_profile_sensor_data) - else: - self.get_logger().warn("DogtoothStatus message not available, skipping dogtooth status subscription.") - def get_git_info(self, repo_path="."): + 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: - # Get current commit hash - commit_hash = subprocess.check_output( - ["git", "rev-parse", "HEAD"], cwd=repo_path - ).decode().strip() - - # Get current commit message - commit_message = subprocess.check_output( - ["git", "log", "-1", "--pretty=%B"], cwd=repo_path - ).decode().strip() - - # Get current commit author - commit_author = subprocess.check_output( - ["git", "log", "-1", "--pretty=%an"], cwd=repo_path - ).decode().strip() - - # Get current commit date - commit_date = subprocess.check_output( - ["git", "log", "-1", "--pretty=%ct"], cwd=repo_path - ).decode().strip() - commit_date = datetime.fromtimestamp(int(commit_date)).strftime('%Y-%m-%d %H:%M:%S') - - # Get current branch name - branch_name = subprocess.check_output( - ["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=repo_path - ).decode().strip() - - # Get remote URL - try: - remote_url = subprocess.check_output( - ["git", "config", "--get", "remote.origin.url"], cwd=repo_path - ).decode().strip() - except subprocess.CalledProcessError: - remote_url = "No remote found" - - git_info = { - "build_version": commit_hash[:7], # Short hash of the latest commit - "commit_message": commit_message, # Commit message - "commit_author": commit_author, # Commit author - "commit_date": commit_date, # Commit date - "branch_name": branch_name, # Current branch name - "remote_url": remote_url # Remote URL - } + val = get_nested_field(msg, mode_field) + except Exception: + 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 - return git_info - except subprocess.CalledProcessError: - return { - "build_version": "Invalid repository", - "commit_message": "", - "commit_author": "", - "commit_date": "", - "branch_name": "Unknown", - "remote_url": "Unknown" + current_time = datetime.now() + + 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 + + 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 } + self.get_logger().debug( + f"[ModeObserver] Received command on {topic_name}, " + f"marking recent autonomous command." + ) - def log_event(self, msg='', details={}): - event_time = datetime.now(tz=timezone.utc) - event = { - 'time': event_time, - 'event_type': msg, - 'details': details, - } - self.db_mgr.add_event(event) + data = {} + log_all = bool(cfg.get("log_all_fields", False)) - # Publish the total number of incidents - incidents_msg = Int8() - incidents_msg.data = self.incidents - self.incidents_publisher.publish(incidents_msg) + 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 - # Also update MDBI and other metrics - self.db_mgr.update_distance(self.distance) - self.db_mgr.update_incidents(self.incidents) - self.db_mgr.update_autonomous_distance(self.autonomous_distance) + 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__})" + ) - self.mdbi = float(self.distance) / float(self.incidents) if self.incidents != 0 else 0.0 - self.db_mgr.update_mdbi(self.mdbi) + 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: + return + + if now is None: + now = self.get_clock().now() - def battery_level_callback(self, msg): - self.details['battery_voltage'] = msg.data - battery_msg = Float32() - battery_msg.data = msg.data - self.battery_publisher.publish(battery_msg) - self.get_logger().debug(f"Battery level published: {msg.data}") + prev_mode = self.details.get('operation_mode', self.MAN) + new_mode = prev_mode - def estop_sub_callback(self, msg): - if msg.data != self.details['estop']: - self.details['estop'] = msg.data - self.get_logger().info(f"Estop status changed to: {self.details['estop']}") + 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 - if self.details['estop']: - self.incidents += 1 - self.get_logger().info(f"Incident count incremented to: {self.incidents}") - self.log_event('EMS', self.details) + self.get_logger().debug( + f"[ModeObserver] is_moving={is_moving}, auto_cmd_recent={auto_cmd_recent}, " + f"prev_mode={prev_mode}" + ) + + if not is_moving: + # Don't flip modes just because we stopped; keep previous + return + + if auto_cmd_recent: + new_mode = self.AUTO + else: + new_mode = self.MAN + + if new_mode == prev_mode: + return + + self.get_logger().info(f"[ModeObserver] Observed mode change: {prev_mode} -> {new_mode}") - def control_mode_callback(self, msg): current_time = datetime.now() - - if msg.data != self.details['operation_mode']: - previous_mode = self.details['operation_mode'] - self.details['operation_mode'] = msg.data - self.get_logger().info(f"operation_mode : {msg.data}") - - # If switching from AUTO to MANUAL, calculate elapsed time in AUTO mode - if previous_mode == self.AUTO and self.details['operation_mode'] == self.MAN: - if self.autonomous_start_time is not None: - elapsed_autonomous_time = (current_time - self.autonomous_start_time).total_seconds() - self.autonomous_time += elapsed_autonomous_time - self.details['autonomous_time'] = self.autonomous_time - self.get_logger().info(f"Autonomous mode elapsed time: {elapsed_autonomous_time} seconds") - self.autonomous_start_time = None # Reset the start time - - # If switching to AUTO mode, start timing - elif self.details['operation_mode'] == self.AUTO: - self.autonomous_start_time = current_time - self.get_logger().info(f"Autonomous mode started at: {self.autonomous_start_time}") - - # Log the event - if self.details['operation_mode'] == self.MAN: - self.incidents += 1 - self.log_event('Manual_override', self.details) - elif self.details['operation_mode'] == self.AUTO: - self.log_event('Autonomous', self.details) - - def coordinator_callback(self, msg): - edge_id = msg.route.edge_id - source = msg.route.source - self.tasks_received_from_coordinator += 1 - self.get_logger().info(f"Task count incremented to: {self.tasks_received_from_coordinator}") - coordinator = {'edge_id': edge_id, 'source': source, 'tasks_received_from_coordinator': self.tasks_received_from_coordinator} - self.details['coordinator'] = coordinator - self.log_event('Coordinator_task', self.details) - - def hunter_status_callback(self, msg): - # Serialize HunterStatus into a dictionary - serialized_data = { - 'header': { - 'stamp': { - 'sec': msg.header.stamp.sec, - 'nanosec': msg.header.stamp.nanosec, - }, - 'frame_id': msg.header.frame_id - }, - 'linear_velocity': msg.linear_velocity, - 'steering_angle': msg.steering_angle, - 'vehicle_state': msg.vehicle_state, - 'control_mode': msg.control_mode, - 'error_code': msg.error_code, - 'battery_voltage': msg.battery_voltage, - 'actuator_states': [ - { - 'motor_id': actuator.motor_id, - 'rpm': actuator.rpm, - 'current': actuator.current, - 'pulse_count': actuator.pulse_count, - 'driver_voltage': actuator.driver_voltage, - 'driver_temperature': actuator.driver_temperature, - 'motor_temperature': actuator.motor_temperature, - 'driver_state': actuator.driver_state - } for actuator in msg.actuator_states - ] - } - self.details['hunter_status'] = serialized_data - - battery = Float32() - battery.data = msg.battery_voltage - self.battery_level_callback(battery) - - control_mode = String() - if msg.control_mode == 3: #this is manual mode in hunter - control_mode.data = self.MAN - elif msg.control_mode == 1 or msg.control_mode == 0: #this is auto mode in hunter - control_mode.data = self.AUTO - self.control_mode_callback(control_mode) - - def dogtooth_status_callback(self, msg): - # Serialize HunterStatus into a dictionary - serialized_data = { - 'header': { - 'stamp': { - 'sec': msg.header.stamp.sec, - 'nanosec': msg.header.stamp.nanosec, - }, - 'frame_id': msg.header.frame_id - }, - 'linear_velocity': msg.linear_velocity, - 'angular_velocity': msg.angular_velocity, - 'vehicle_state': msg.vehicle_state, - 'control_mode': msg.control_mode, - 'error_code': msg.error_code, - 'battery_voltage': msg.battery_voltage, + + # 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 + + 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.system_snapshot['cmd_vel_nav'] = { + 'linear_x': msg.linear.x, + 'linear_y': msg.linear.y, + 'angular_z': msg.angular.z, } - self.details['dogtooth_status'] = serialized_data - - battery = Float32() - battery.data = msg.battery_voltage - self.battery_level_callback(battery) - - control_mode = String() - if msg.control_mode == 3: #this is manual mode in hunter - control_mode.data = self.MAN - elif msg.control_mode == 1 or msg.control_mode == 0: #this is auto mode in hunter - control_mode.data = self.AUTO - self.control_mode_callback(control_mode) - - def gps_fix_callback(self, msg): - gps_data = { - 'latitude': msg.latitude, - 'longitude': msg.longitude, - 'altitude': msg.altitude, - 'position_covariance': list(msg.position_covariance), - 'position_covariance_type': msg.position_covariance_type, - 'status': msg.status.status, - 'service': msg.status.service + + 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.details['gps_data'] = gps_data + 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." + ) + collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold + self.collision_prev_has_velocity = collision_has_velocity_now + return - if not self.first_gps_fix_received: - self.first_gps_fix_received = True - self.log_event('First_GNSS_msg', self.details) + # 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 + return - def gps_odom_callback(self, msg): - position = msg.pose.pose.position + nav_vx = float(self.last_nav_cmd.linear.x) - if self._initialize_pose(position): + # 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." + ) + collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold + self.collision_prev_has_velocity = collision_has_velocity_now return - distance = self._calculate_distance(position) + # 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}" + ) + + # 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.speed = 0.0 + self.publish_speed(0.0) - if self._is_significant_distance(distance): - self._update_distances(distance, position) + 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 _initialize_pose(self, position): - if self.init_pose: - self.init_pose = False - self.previous_x = position.x - self.previous_y = position.y - return True - return False + 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}" + ) - def _calculate_distance(self, position): - dx = position.x - self.previous_x - dy = position.y - self.previous_y - return math.sqrt(dx**2 + dy**2) + db_managers = [self.db_mgr_local] + if self.db_mgr_remote: + db_managers.append(self.db_mgr_remote) - def _is_significant_distance(self, distance): - return distance >= self.min_distance_threshold + 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() + } - def _update_distances(self, distance, position): - current_time = self.get_clock().now() + event_time = datetime.now(tz=timezone.utc) + event = {'time': event_time, 'event_type': msg, 'details': details} - # Calculate time difference between the last update and now - if hasattr(self, 'previous_time'): - time_diff = (current_time - self.previous_time).nanoseconds * 1e-9 # Convert to seconds - self.speed = distance / time_diff if time_diff > 0 else 0.0 - else: - self.speed = 0.0 + self.get_logger().info( + f"[Event] type={msg}, time={event_time.isoformat()}, " + f"incidents={self.incidents}, collisions={self.collision_incidents}" + ) - # Update the previous time - self.previous_time = current_time + 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}") - # Update the total traveled distance - self.distance += distance - if self.details['operation_mode'] == self.AUTO: - self.autonomous_distance += distance - self.previous_x = position.x - self.previous_y = position.y + incidents_msg = Int32() + incidents_msg.data = self.incidents + self.incidents_publisher.publish(incidents_msg) - # Publish the distance and speed - self.publish_distance(self.distance) - self.publish_speed(self.speed) + 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), + }, + "angular": { + "z": float(self.last_nav_cmd.angular.z), + }, + }, + "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), + }, + }, + # 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']})" + ) - def publish_distance(self, distance): - # Publish total traveled distance - distance_msg = Float32() - distance_msg.data = distance - self.distance_publisher.publish(distance_msg) + # This will attach metrics (distance, speed, battery, etc.) and write to DB + self.log_event("Collision", details) - self.db_mgr.update_distance(self.distance) - self.db_mgr.update_autonomous_distance(self.autonomous_distance) + # Publish total collisions on a dedicated topic + msg = Int32() + msg.data = int(self.collision_incidents) + self.collision_incidents_publisher.publish(msg) - # Log the speed and distance - self.get_logger().debug(f"Traveled distance: {self.distance} meters") + 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): - # Publish robot speed - speed_msg = Float32() - speed_msg.data = speed - self.speed_publisher.publish(speed_msg) + 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 + } + """ + 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, + } + 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_distance(self, distance): - self.get_logger().debug(f"distance: {distance}") + try: + info["short_commit"] = git(["rev-parse", "--short", "HEAD"]) + except Exception: + info["short_commit"] = None + + # NEW: Last commit message (full body or just subject) + try: + info["commit_message"] = git(["log", "-1", "--pretty=%s"]) + except Exception: + info["commit_message"] = None + + # 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 + + # Dirty / clean state + try: + status = git(["status", "--porcelain"]) + info["dirty"] = bool(status) + except Exception: + info["dirty"] = None + + except Exception as e: + info["error"] = str(e) + return info def main(args=None): rclpy.init(args=args) - - autonomy_metric_logger = AutonomyMetricsLogger() - - rclpy.spin(autonomy_metric_logger) - autonomy_metric_logger.destroy_node() - rclpy.shutdown() + node = AutonomyMetricsLogger() + try: + rclpy.spin(node) + except KeyboardInterrupt: + node.get_logger().info("Shutting down AutonomyMetricsLogger (KeyboardInterrupt).") + 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() if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/src/autonomy_metrics/config/metrics_full.yaml b/src/autonomy_metrics/config/metrics_full.yaml new file mode 100644 index 0000000..c210be9 --- /dev/null +++ b/src/autonomy_metrics/config/metrics_full.yaml @@ -0,0 +1,127 @@ +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" + perception_boundary_estimator: "jabas/perception_boundary_estimator" + pointcloud_processing: "jabas/pointcloud_processing" + robot_boundary_checker: "jabas/robot_boundary_checker" + ros2_topic_monitor: "jabas/ros2_topic_monitor" + row_nav_planner: "jabas/row_nav_planner" + 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 + + # Tell logger which field carries battery level so it can be attached + battery_field: "battery_percentage" + + 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" + + # --------------------------------------------------------- + # 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 + + + # --------------------------------------------------------- + # 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" + + # --------------------------------------------------------- + # 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: "/cmd_vel/collision" + type: "geometry_msgs/msg/Twist" + role: "collision_output" + log_fields: [] # no per-message events needed + + # --------------------------------------------------------- + # 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: "/gophar/motor_status/motor_0" + type: "dynium_gophar_interfaces/msg/MotorStatus" + log_all_fields: true + + - name: "/gophar/motor_status/motor_1" + type: "dynium_gophar_interfaces/msg/MotorStatus" + log_all_fields: true \ No newline at end of file diff --git a/src/autonomy_metrics/launch/autonomy_metrics.launch.py b/src/autonomy_metrics/launch/autonomy_metrics.launch.py index 0844fdd..91f5984 100644 --- a/src/autonomy_metrics/launch/autonomy_metrics.launch.py +++ b/src/autonomy_metrics/launch/autonomy_metrics.launch.py @@ -1,75 +1,167 @@ -# -*- coding: utf-8 -*- -#! /usr/bin/env python3 +#!/usr/bin/env python3 import os from launch import LaunchDescription -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - - gps_topic = LaunchConfiguration('gps_topic', default='/gps_base/fix') - odometry_topic = LaunchConfiguration('odometry_topic', default='/diff_drive_controller/odom') - battery_status = LaunchConfiguration('battery_status_topic', default='/diff_drive_controller/battery_status') - estop_status = LaunchConfiguration('estop_status_topic', default='/diff_drive_controller/estop_status') - hunter_status = LaunchConfiguration('hunter_status_topic', default='/hunter_status') - actioned_by_coordinator_topic = LaunchConfiguration('actioned_by_coordinator_topic', default='/topological_navigation/execute_policy_mode/goal') + # ------------------------------------------------------------------------- + # Launch arguments (all node parameters exposed) + # ------------------------------------------------------------------------- + + pkg_dir = get_package_share_directory('autonomy_metrics') - min_distance_threshold = LaunchConfiguration('min_distance_threshold', default='0.2') + 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, - # Path to the scenario and navigation repos - aoc_scenario_path = LaunchConfiguration('aoc_scenario_path', default='/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario') - aoc_navigation_path = LaunchConfiguration('aoc_navigation_path', default='/home/ros/aoc_strawberry_scenario_ws/src/aoc_navigation') + 'mode_observer_cmd_timeout': mode_observer_cmd_timeout, + 'mode_observer_speed_threshold': mode_observer_speed_threshold, - # New parameters for MongoDB host and port - mongodb_host = LaunchConfiguration('mongodb_host', default='localhost') - mongodb_port = LaunchConfiguration('mongodb_port', default=os.getenv('MONGOD_PORT', '27017')) + 'collision_nav_threshold': collision_nav_threshold, + 'collision_zero_threshold': collision_zero_threshold, + 'collision_time_window': collision_time_window, + 'collision_log_cooldown': collision_log_cooldown, + }], + ) return LaunchDescription([ - # Declare Launch Arguments for the existing parameters - DeclareLaunchArgument( - 'gps_topic', default_value=gps_topic, description='GPS topic for base fix'), - DeclareLaunchArgument( - 'odometry_topic', default_value=odometry_topic, description='A topic for odometry'), - DeclareLaunchArgument( - 'battery_status_topic', default_value=battery_status, description='Battery status topic'), - DeclareLaunchArgument( - 'estop_status_topic', default_value=estop_status, description='E-stop status topic'), - DeclareLaunchArgument( - 'hunter_status_topic', default_value=hunter_status, description='hunter status topic'), - DeclareLaunchArgument( - 'actioned_by_coordinator_topic', default_value=actioned_by_coordinator_topic, description=''), - - DeclareLaunchArgument( - 'min_distance_threshold', default_value=min_distance_threshold, description=''), - - DeclareLaunchArgument( - 'aoc_scenario_path', default_value=aoc_scenario_path, description='path to aoc scenario'), - DeclareLaunchArgument( - 'aoc_navigation_path', default_value=aoc_navigation_path, description='path to aoc navigation'), - - # Declare Launch Arguments for the new MongoDB parameters - DeclareLaunchArgument( - 'mongodb_host', default_value=mongodb_host, description='MongoDB host address'), - DeclareLaunchArgument( - 'mongodb_port', default_value=mongodb_port, description='MongoDB port'), - - # Node Configuration - Node( - package='autonomy_metrics', - executable='metric_logger', - output='screen', - parameters=[{ - 'gps_topic': gps_topic, - 'odometry_topic': odometry_topic, - 'battery_status_topic': battery_status, - 'estop_status_topic': estop_status, - 'hunter_status_topic': hunter_status, - 'aoc_scenario_path': aoc_scenario_path, - 'aoc_navigation_path': aoc_navigation_path, - 'mongodb_host': mongodb_host, - 'mongodb_port': mongodb_port, - }], - ) + 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, ])