From fa23e4cd11517dd95fafd0484b680835a74c095e Mon Sep 17 00:00:00 2001 From: ibrahim Date: Sat, 28 Feb 2026 22:27:25 +0000 Subject: [PATCH 1/3] Enhance collision detection and intervention triggers in AutonomyMetricsLogger - Added parameters for collision moving threshold and required zero count. - Improved collision detection logic with hysteresis and debounce. - Updated intervention triggers to require autonomous mode and added activity field checks. - Enhanced YAML configuration for intervention on message with new parameters. --- README.md | 63 +++++- .../autonomy_metrics/metric_logger.py | 192 +++++++++++++++--- src/autonomy_metrics/config/metrics_full.yaml | 5 + 3 files changed, 226 insertions(+), 34 deletions(-) diff --git a/README.md b/README.md index c62fcc8..f7a56b3 100644 --- a/README.md +++ b/README.md @@ -156,18 +156,25 @@ Logic when active: | -------------------------- | ------ | ------- | --------------------------------------------------------------------------------------------- | | `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_moving_threshold` | double | `0.03` | `/cmd_vel/collision.linear.x` considered moving if abs(x) ≥ this (hysteresis upper bound). | +| `collision_zero_required_count` | int | `2` | Require this many consecutive near-zero collision samples before counting a collision. | | `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_log_cooldown` | double | `1.0` | Minimum time between logged collision events. | +| `intervention_event_cooldown` | double | `1.0` | Default cooldown for YAML `intervention_on_message` triggers. | -Collision detection (falling-edge): +Collision detection (robust 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, + * Current mode is `Autonomous`, + * Previous collision stream state was moving (`>= collision_moving_threshold`), + * Current collision stream is near-zero (`<= collision_zero_threshold`) for `collision_zero_required_count` consecutive samples, * Latest `/cmd_vel/nav.linear.x` > `collision_nav_threshold`, * Commands are recent (`≤ collision_time_window`). +* Additional safeguard: + + * Events are rate-limited by `collision_log_cooldown`. * On collision: * `collision_incidents` is incremented, @@ -244,6 +251,11 @@ topics: intervention_on_message: enable: true event_type: "Joy_override" + require_autonomous_mode: true + on_rising_edge: true + cooldown_sec: 1.0 + activity_field: "linear.x" + min_abs_value: 0.02 - name: "/cmd_vel/collision_smoothed" type: "geometry_msgs/msg/Twist" @@ -370,9 +382,22 @@ Per-topic: intervention_on_message: enable: true event_type: "Joy_override" + require_autonomous_mode: true + on_rising_edge: true + cooldown_sec: 1.0 + activity_field: "linear.x" + min_abs_value: 0.02 ``` - If enabled, **every message** on that topic triggers an event of this type via `trigger_intervention`. + If enabled, events are emitted only when trigger conditions pass: + + * `require_autonomous_mode` (default `true`): skip in Manual mode. + * `on_rising_edge` (default `true`): trigger only on inactive→active transition. + * `cooldown_sec` (default from node param `intervention_event_cooldown`): per-trigger rate limit. + * `activity_field` (default `linear.x`): field used to determine activity. + * `min_abs_value`: minimum absolute value for numeric activity fields. + + This avoids noisy per-message logs and false overrides from zero/idle commands. * `intervention_on_change`: @@ -408,9 +433,32 @@ Per-topic: * E-stop, faults, joy overrides, etc. in **Manual** mode: - * Are logged to DB (event + snapshot), + * Are suppressed by default for intervention triggers, + * Can still be explicitly logged for critical events (e.g. E-stop), * Do **not** increment `incidents`, thus do not affect MDBI. +### Recommended anti-noise presets + +Simulation (cleaner signals): + +* `mode_observer_speed_threshold: 0.01` +* `collision_nav_threshold: 0.01` +* `collision_zero_threshold: 0.001` +* `collision_moving_threshold: 0.02` +* `collision_zero_required_count: 2` +* `collision_log_cooldown: 0.6` +* `intervention_event_cooldown: 0.8` + +Real robot (noisier signals): + +* `mode_observer_speed_threshold: 0.02` +* `collision_nav_threshold: 0.03` +* `collision_zero_threshold: 0.01` +* `collision_moving_threshold: 0.05` +* `collision_zero_required_count: 3` +* `collision_log_cooldown: 1.5` +* `intervention_event_cooldown: 1.5` + --- ## 6. Launching the Node @@ -452,8 +500,11 @@ def generate_launch_description(): 'mode_observer_speed_threshold': LaunchConfiguration('mode_observer_speed_threshold'), 'collision_nav_threshold': LaunchConfiguration('collision_nav_threshold'), 'collision_zero_threshold': LaunchConfiguration('collision_zero_threshold'), + 'collision_moving_threshold': LaunchConfiguration('collision_moving_threshold'), + 'collision_zero_required_count': LaunchConfiguration('collision_zero_required_count'), 'collision_time_window': LaunchConfiguration('collision_time_window'), 'collision_log_cooldown': LaunchConfiguration('collision_log_cooldown'), + 'intervention_event_cooldown': LaunchConfiguration('intervention_event_cooldown'), }], ) diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index d6573a6..bc583ef 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -115,13 +115,19 @@ def __init__(self): # 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_moving_threshold', 0.03) # collision cmd abs(linear.x) >= this to consider as "has velocity" + self.declare_parameter('collision_zero_required_count', 2) # require N consecutive near-zero collision commands before logging 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.declare_parameter('intervention_event_cooldown', 1.0) # default cooldown for YAML intervention triggers 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_moving_threshold = self.get_parameter('collision_moving_threshold').get_parameter_value().double_value + self.collision_zero_required_count = self.get_parameter('collision_zero_required_count').get_parameter_value().integer_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.intervention_event_cooldown = self.get_parameter('intervention_event_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 @@ -136,7 +142,9 @@ def __init__(self): 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}" + f"zero_thr={self.collision_zero_threshold}, moving_thr={self.collision_moving_threshold}, " + f"zero_required={self.collision_zero_required_count}, time_window={self.collision_time_window}, " + f"cooldown={self.collision_log_cooldown}" ) # DB Managers @@ -172,6 +180,7 @@ def __init__(self): # Collision monitoring self.collision_incidents = 0 self.collision_prev_has_velocity = False # for falling-edge detection + self.collision_zero_streak = 0 self.last_nav_cmd = None self.last_nav_time = None @@ -187,6 +196,8 @@ def __init__(self): # Snapshot of latest values per field for change detection self.prev_field_values = {} + self.prev_message_trigger_active = {} + self.last_intervention_event_time = {} # Current battery level (updated from any topic with 'battery_field') self.current_battery = None @@ -352,7 +363,13 @@ def get_metrics_snapshot(self): metrics["battery_percentage"] = self.current_battery return metrics - def trigger_intervention(self, event_type: str, extra: dict | None = None, force_count: bool = False): + def trigger_intervention( + self, + event_type: str, + extra: dict | None = None, + force_count: bool = False, + log_when_not_counted: bool = False, + ): """ Log an intervention-type event. @@ -368,6 +385,12 @@ def trigger_intervention(self, event_type: str, extra: dict | None = None, force if count_incident: self.incidents += 1 + elif not log_when_not_counted: + self.get_logger().debug( + f"[Intervention] Suppressed event '{event_type}' in mode={current_mode} " + "(not counted and log_when_not_counted=False)." + ) + return self.get_logger().info( f"[Intervention] type={event_type}, mode={current_mode}, " @@ -380,20 +403,84 @@ def trigger_intervention(self, event_type: str, extra: dict | None = None, force self.log_event(event_type, base_details) - def handle_intervention_triggers(self, topic_name: str, data: dict, cfg: dict): + def handle_intervention_triggers(self, topic_name: str, msg, 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) + now = self.get_clock().now() + + # 1) Trigger on message activity (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}) + require_auto = msg_trig.get("require_autonomous_mode", True) + on_rising_edge = msg_trig.get("on_rising_edge", True) + cooldown_sec = float(msg_trig.get("cooldown_sec", self.intervention_event_cooldown)) + + activity_field = msg_trig.get("activity_field", "linear.x") + min_abs_cfg = msg_trig.get("min_abs_value", None) + if min_abs_cfg is None: + if activity_field.startswith("linear.") or activity_field.startswith("angular."): + min_abs_value = float(self.mode_observer_speed_threshold) + else: + min_abs_value = 0.0 + else: + min_abs_value = float(min_abs_cfg) + + activity_value = None + try: + activity_value = get_nested_field(msg, activity_field) + except Exception: + if "data" in data: + activity_value = data["data"] + + if isinstance(activity_value, bool): + is_active = activity_value + elif isinstance(activity_value, (int, float)): + is_active = abs(float(activity_value)) >= min_abs_value + elif activity_value is None: + # If we cannot evaluate activity, treat any message as activity. + is_active = True + else: + is_active = bool(activity_value) + + trig_key = (topic_name, evt_type) + prev_active = self.prev_message_trigger_active.get(trig_key, False) + self.prev_message_trigger_active[trig_key] = is_active + + current_mode = self.details.get('operation_mode', self.MAN) + if require_auto and current_mode != self.AUTO: + self.get_logger().debug( + f"[Trigger] Skip '{evt_type}' on '{topic_name}' because mode={current_mode}" + ) + else: + should_fire = is_active and (not on_rising_edge or not prev_active) + if should_fire: + last_event_time = self.last_intervention_event_time.get(trig_key) + if last_event_time is not None: + dt = (now - last_event_time).nanoseconds * 1e-9 + if dt < cooldown_sec: + should_fire = False + self.get_logger().debug( + f"[Trigger] Cooldown active for '{evt_type}' ({dt:.3f}s < {cooldown_sec:.3f}s)" + ) + + if should_fire: + self.last_intervention_event_time[trig_key] = now + self.get_logger().info( + f"[Trigger] message_activity on '{topic_name}' -> event '{evt_type}', " + f"value={activity_value}, active={is_active}" + ) + self.trigger_intervention( + evt_type, + extra={ + "topic": topic_name, + "activity_field": activity_field, + "activity_value": activity_value, + }, + ) # 2) Trigger on change of specific fields field_trigs = cfg.get("intervention_on_change", {}) @@ -404,9 +491,14 @@ def handle_intervention_triggers(self, topic_name: str, data: dict, cfg: dict): 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 + # First sample establishes baseline; don't emit a change event on startup. + if prev is None: + continue + + changed = (prev != new) + if not changed: continue @@ -561,7 +653,7 @@ def estop_role_callback(self, topic_name, msg): self.get_logger().info(f"E-Stop state changed: {v}") if v: # E-stop counts as intervention - self.trigger_intervention('EMS') + self.trigger_intervention('EMS', log_when_not_counted=True) def generic_callback(self, topic_name, msg, log_fields): cfg = self.topic_cfg_map.get(topic_name, {}) @@ -612,7 +704,7 @@ def generic_callback(self, topic_name, msg, log_fields): ) # Apply YAML-configured intervention triggers (message & field-change) - self.handle_intervention_triggers(topic_name, data, cfg) + self.handle_intervention_triggers(topic_name, msg, data, cfg) # Dynamic republishing if configured self.handle_dynamic_publish(topic_name, msg) @@ -784,13 +876,27 @@ def check_collision_condition(self, now, coll_vx_now: float): We only increment again if the cycle repeats: velocity on /cmd_vel/collision -> zero -> velocity -> zero -> ... """ + # Only count collisions while the robot is in autonomous mode. + if self.details.get('operation_mode') != self.AUTO: + collision_abs_vx = abs(coll_vx_now) + if collision_abs_vx >= self.collision_moving_threshold: + self.collision_prev_has_velocity = True + elif collision_abs_vx <= self.collision_zero_threshold: + self.collision_prev_has_velocity = False + self.collision_zero_streak = 0 + return + # 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 + collision_abs_vx = abs(coll_vx_now) + if collision_abs_vx >= self.collision_moving_threshold: + self.collision_prev_has_velocity = True + elif collision_abs_vx <= self.collision_zero_threshold: + self.collision_prev_has_velocity = False + self.collision_zero_streak = 0 return # Check nav recency @@ -800,8 +906,12 @@ def check_collision_condition(self, now, coll_vx_now: float): 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 + collision_abs_vx = abs(coll_vx_now) + if collision_abs_vx >= self.collision_moving_threshold: + self.collision_prev_has_velocity = True + elif collision_abs_vx <= self.collision_zero_threshold: + self.collision_prev_has_velocity = False + self.collision_zero_streak = 0 return nav_vx = float(self.last_nav_cmd.linear.x) @@ -812,31 +922,57 @@ def check_collision_condition(self, now, coll_vx_now: float): 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 + collision_abs_vx = abs(coll_vx_now) + if collision_abs_vx >= self.collision_moving_threshold: + self.collision_prev_has_velocity = True + elif collision_abs_vx <= self.collision_zero_threshold: + self.collision_prev_has_velocity = False + self.collision_zero_streak = 0 return - # Edge detection on /cmd_vel/collision - collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold + # Hysteresis-based state on /cmd_vel/collision + collision_abs_vx = abs(coll_vx_now) + collision_is_zero_now = collision_abs_vx <= self.collision_zero_threshold + collision_has_velocity_now = collision_abs_vx >= self.collision_moving_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}" + f"now_has_vel={collision_has_velocity_now}, now_is_zero={collision_is_zero_now}, " + f"coll_vx_now={coll_vx_now:.3f}, nav_vx={nav_vx:.3f}, zero_streak={self.collision_zero_streak}" ) - # Falling edge: had velocity before, now ~zero - if self.collision_prev_has_velocity and not collision_has_velocity_now: - # This is one collision incident + if collision_has_velocity_now: + self.collision_prev_has_velocity = True + self.collision_zero_streak = 0 + return + + if not collision_is_zero_now: + # In hysteresis band; keep previous state and wait for clearer signal. + return + + self.collision_zero_streak += 1 + + # Falling edge with debounce: had velocity before, now zero for N consecutive samples. + if self.collision_prev_has_velocity and self.collision_zero_streak >= int(self.collision_zero_required_count): + if self.last_collision_logged_time is not None: + dt_coll = (now - self.last_collision_logged_time).nanoseconds * 1e-9 + if dt_coll < self.collision_log_cooldown: + self.get_logger().debug( + f"[Collision] Cooldown active ({dt_coll:.3f}s < {self.collision_log_cooldown:.3f}s); skip log." + ) + self.collision_prev_has_velocity = False + self.collision_zero_streak = 0 + return + self.collision_incidents += 1 + self.last_collision_logged_time = now 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})" + f"(nav_vx={nav_vx:.3f}, coll_vx_now={coll_vx_now:.3f}, zero_count={self.collision_zero_streak})" ) self.log_collision_event() - - # Update for next call - self.collision_prev_has_velocity = collision_has_velocity_now + self.collision_prev_has_velocity = False + self.collision_zero_streak = 0 # ------------------------- # Timer & Logging diff --git a/src/autonomy_metrics/config/metrics_full.yaml b/src/autonomy_metrics/config/metrics_full.yaml index c210be9..ab97e67 100644 --- a/src/autonomy_metrics/config/metrics_full.yaml +++ b/src/autonomy_metrics/config/metrics_full.yaml @@ -93,6 +93,11 @@ topics: intervention_on_message: enable: true event_type: "Joy_override" + require_autonomous_mode: true + on_rising_edge: true + cooldown_sec: 1.0 + activity_field: "linear.x" + min_abs_value: 0.02 # --------------------------------------------------------- # Collision monitor: nav vs collision-filtered velocity From 5a2ee592218dcc5bbc68ab3767c3393096418714 Mon Sep 17 00:00:00 2001 From: ibrahim Date: Thu, 5 Mar 2026 11:44:45 +0000 Subject: [PATCH 2/3] Add navigation area tracking and path deviation computation to AutonomyMetricsLogger - Implement NavigationAreaTracker for monitoring robot's navigation area and deriving average speeds and status. - Introduce PathDeviationTracker for calculating cross-track error from the global path. - Update README.md to reflect new features and configuration options. - Enhance metric logging with periodic snapshots that include navigation area and path deviation data. --- README.md | 137 ++++++++++- .../autonomy_metrics/metric_logger.py | 80 ++++++- .../navigation_area_tracker.py | 216 ++++++++++++++++++ .../path_deviation_tracker.py | 115 ++++++++++ src/autonomy_metrics/config/metrics_full.yaml | 29 ++- 5 files changed, 571 insertions(+), 6 deletions(-) create mode 100644 src/autonomy_metrics/autonomy_metrics/navigation_area_tracker.py create mode 100644 src/autonomy_metrics/autonomy_metrics/path_deviation_tracker.py diff --git a/README.md b/README.md index f7a56b3..216c9d3 100644 --- a/README.md +++ b/README.md @@ -6,6 +6,10 @@ - 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). +- Tracks navigation area (in-row / headland / row-entry) with per-area average speed. +- Derives a high-level robot status (In-row, Headland, Row-entry, Paused due to obstacle, Held by picker). +- Computes cross-track deviation from the global path. +- Periodically logs a full system snapshot to MongoDB (`timer_event`, not counted as MDBI incident). --- @@ -26,7 +30,8 @@ The node: - E-stop - Faults / joy overrides / other YAML triggers - Collisions (nav vs collision velocity) -6. Each event stored in Mongo includes: +6. Periodically logs a `timer_event` (default every 5 s) with the full system snapshot to MongoDB. These events are **not** counted as incidents and do **not** affect MDBI. +7. Each event stored in Mongo includes: - `metrics` (distance, autonomous_distance, speed, battery) - `system_snapshot` (current values of all configured topics) - Event type & extra details. @@ -128,6 +133,7 @@ All parameters are exposed through the launch file. | `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. | +| `snapshot_timer_interval` | double | `5.0` | Interval (s) between periodic `timer_event` snapshots logged to MongoDB. | ### Mode observer (fallback mode inference) @@ -291,6 +297,24 @@ topics: type: "dynium_gophar_interfaces/msg/MotorStatus" log_all_fields: true log_fields: [] + + # Navigation area, execution UI, and global path + - name: "/robot_navigation_area" + type: "std_msgs/msg/String" + role: "navigation_area" + log_fields: + - "data" + + - name: "/roboflow/execution_ui" + type: "std_msgs/msg/String" + role: "execution_ui" + log_fields: + - "data" + + - name: "/plan" + type: "nav_msgs/msg/Path" + role: "global_path" + log_fields: [] ``` ### Per-topic keys @@ -316,6 +340,9 @@ Common fields: * `"mode_observer"`: used by mode observer (e.g. `/cmd_vel/collision_smoothed`). * `"collision_nav"`: nav velocity command. * `"collision_output"`: collision-limited velocity command. + * `"navigation_area"`: navigation area enum (e.g. `/robot_navigation_area`). + * `"execution_ui"`: execution UI state (`idle` / `paused`, e.g. `/roboflow/execution_ui`). + * `"global_path"`: global planner path (`nav_msgs/msg/Path`) used for path-deviation error. * `log_fields` (list of dotted paths) Fields to extract into `system_snapshot[topic_name]`. @@ -414,7 +441,66 @@ Per-topic: --- -## 5. Mode Handling and Incidents +## 5. Navigation Area, Status & Path Deviation + +These features are implemented in two modular tracker classes that are instantiated by the main node. + +### Navigation area tracking (`NavigationAreaTracker`) + +Subscribes (via `role: "navigation_area"`) to a topic publishing `std_msgs/msg/String` with one of: + +* `INSIDE_POLYTUNNEL` +* `OUTSIDE_POLYTUNNEL` +* `TRANSITION_INTO_POLYTUNNEL` + +Also monitors the execution UI topic (`role: "execution_ui"`) for values `idle` or `paused`. + +#### Published topics + +| Topic | Type | Description | +| ------------------------------------------- | ------- | -------------------------------------------------------- | +| `mdbi_logger/avg_speed_inside_polytunnel` | Float32 | Running average speed while inside a polytunnel. | +| `mdbi_logger/avg_speed_outside_polytunnel` | Float32 | Running average speed while outside a polytunnel. | +| `mdbi_logger/time_in_current_mode` | Float32 | Seconds since the last area transition (resets on change).| +| `mdbi_logger/robot_status` | String | Derived high-level status (see below). | +| `mdbi_logger/navigation_area` | String | Current navigation area string. | + +#### Robot status derivation (priority order) + +| Priority | Status | Condition | +| -------- | ----------------------- | ------------------------------------------------------------- | +| 1 | **Held by picker** | `/roboflow/execution_ui` is `"paused"` | +| 2 | **Paused due to obstacle** | Autonomous mode, speed ≈ 0, area is known | +| 3 | **Row-entry** | Area = `TRANSITION_INTO_POLYTUNNEL` | +| 4 | **In-row** | Area = `INSIDE_POLYTUNNEL` | +| 5 | **Headland** | Area = `OUTSIDE_POLYTUNNEL` | +| 6 | **Unknown** | No area data received yet | + +### Path deviation tracking (`PathDeviationTracker`) + +Subscribes (via `role: "global_path"`) to a `nav_msgs/msg/Path` topic (default `/plan`). + +On every odometry update the tracker computes the minimum perpendicular distance from the robot's current position to the closest segment of the received global path. + +| Topic | Type | Description | +| --------------------------------- | ------- | ------------------------------------------ | +| `mdbi_logger/path_deviation_error`| Float32 | Cross-track error to the global path (m). | + +### Periodic snapshot (`timer_event`) + +A timer (configurable via `snapshot_timer_interval`, default 5 s) fires periodically and logs a `timer_event` to MongoDB. This event captures: + +* Full `system_snapshot` (latest values from all subscribed topics) +* Current metrics (distance, autonomous distance, speed, battery) +* Navigation area tracker snapshot (area, status, per-area avg speeds, time-in-mode) +* Path deviation tracker snapshot (error, path length) +* Current `operation_mode` and `estop` state + +**`timer_event` does NOT increment `incidents` and does NOT affect MDBI.** + +--- + +## 6. Mode Handling and Incidents * Default mode at startup is set to `Autonomous`: @@ -461,7 +547,7 @@ Real robot (noisier signals): --- -## 6. Launching the Node +## 7. Launching the Node Example launch file (simplified; adapt package/executable names): @@ -505,6 +591,7 @@ def generate_launch_description(): 'collision_time_window': LaunchConfiguration('collision_time_window'), 'collision_log_cooldown': LaunchConfiguration('collision_log_cooldown'), 'intervention_event_cooldown': LaunchConfiguration('intervention_event_cooldown'), + 'snapshot_timer_interval': LaunchConfiguration('snapshot_timer_interval'), }], ) @@ -545,7 +632,7 @@ ros2 launch autonomy_metrics_logger autonomy_metrics_logger.launch.py \ --- -## 7. Testing Tips +## 8. Testing Tips * Verify topics: @@ -562,7 +649,49 @@ ros2 launch autonomy_metrics_logger autonomy_metrics_logger.launch.py \ "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" ``` +* Publish a navigation area: + + ```bash + ros2 topic pub --once /robot_navigation_area std_msgs/msg/String "{data: 'INSIDE_POLYTUNNEL'}" + ``` + +* Simulate picker hold: + + ```bash + ros2 topic pub --once /roboflow/execution_ui std_msgs/msg/String "{data: 'paused'}" + ``` + +* Verify derived topics: + + ```bash + ros2 topic echo /mdbi_logger/robot_status + ros2 topic echo /mdbi_logger/avg_speed_inside_polytunnel + ros2 topic echo /mdbi_logger/path_deviation_error + ``` + * Check DB content (Mongo shell or Mongo client) in `robot_incidents.sessions`. + Look for events with `event_type: "timer_event"` for periodic snapshots. + +--- + +## 9. Architecture Overview + +The node is structured with modular tracker classes to keep concerns separated: + +``` +AutonomyMetricsLogger (metric_logger.py) +├── NavigationAreaTracker (navigation_area_tracker.py) +│ ├── Area tracking (INSIDE / OUTSIDE / TRANSITION) +│ ├── Per-area average speed +│ ├── Time-in-current-mode +│ └── Robot status derivation +├── PathDeviationTracker (path_deviation_tracker.py) +│ ├── Global path storage +│ └── Cross-track error computation +├── DatabaseMgr (db_mgr.py) +│ └── MongoDB read/write +└── Periodic snapshot timer (timer_event → MongoDB) +``` The README covers: diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index bc583ef..95f7da4 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -18,9 +18,11 @@ 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 std_msgs.msg import Bool, Float32, Int32, String from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr +from autonomy_metrics.navigation_area_tracker import NavigationAreaTracker +from autonomy_metrics.path_deviation_tracker import PathDeviationTracker def ros_msg_to_dict(msg): @@ -120,6 +122,7 @@ def __init__(self): 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.declare_parameter('intervention_event_cooldown', 1.0) # default cooldown for YAML intervention triggers + self.declare_parameter('snapshot_timer_interval', 5.0) # seconds between periodic snapshot events 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 @@ -128,6 +131,7 @@ def __init__(self): 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.intervention_event_cooldown = self.get_parameter('intervention_event_cooldown').get_parameter_value().double_value + self.snapshot_timer_interval = self.get_parameter('snapshot_timer_interval').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 @@ -222,6 +226,17 @@ def __init__(self): self.heartbeat_timer = self.create_timer(1.0, self.timer_callback) + # --- Modular trackers --- + self.nav_area_tracker = NavigationAreaTracker( + self, paused_speed_threshold=self.mode_observer_speed_threshold + ) + self.path_deviation_tracker = PathDeviationTracker(self) + + # Periodic snapshot timer (records all data to MongoDB without affecting MDBI) + self.snapshot_timer = self.create_timer( + self.snapshot_timer_interval, self.periodic_snapshot_callback + ) + self.load_and_setup_config() # Use the observer only if there is no explicit control_mode topic configured @@ -340,6 +355,12 @@ def load_and_setup_config(self): 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) + elif role == 'navigation_area': + cb = lambda msg, n=name: self.navigation_area_callback(n, msg) + elif role == 'execution_ui': + cb = lambda msg, n=name: self.execution_ui_callback(n, msg) + elif role == 'global_path': + cb = lambda msg, n=name: self.global_path_callback(n, msg) else: lf = item.get('log_fields', []) cb = lambda msg, n=name, l=lf: self.generic_callback(n, msg, l) @@ -537,6 +558,9 @@ def odom_role_callback(self, topic_name, msg): except Exception: return + # Always feed the current position to the path-deviation tracker + self.path_deviation_tracker.update_position(pos.x, pos.y) + current_time = self.get_clock().now() if self.init_pose: @@ -595,6 +619,14 @@ def odom_role_callback(self, topic_name, msg): self.publish_speed(self.speed) self.handle_dynamic_publish(topic_name, msg) + # Feed speed to the navigation-area tracker + is_auto = self.details.get('operation_mode') == self.AUTO + self.nav_area_tracker.update_speed(self.speed, is_auto) + + # Include tracker snapshots in the system snapshot + self.system_snapshot['navigation_area'] = self.nav_area_tracker.get_snapshot() + self.system_snapshot['path_deviation'] = self.path_deviation_tracker.get_snapshot() + def control_mode_role_callback(self, topic_name, msg): cfg = self.topic_cfg_map.get(topic_name, {}) mode_field = cfg.get('mode_field', 'data') @@ -747,6 +779,48 @@ def handle_dynamic_publish(self, topic_name, msg): except Exception as e: self.get_logger().error(f"Dynamic publish failed for {topic_name}: {e}") + # ------------------------- + # Navigation Area / Execution UI / Global Path callbacks + # ------------------------- + def navigation_area_callback(self, topic_name, msg): + """Handle /robot_navigation_area messages.""" + area = msg.data.strip() + self.system_snapshot[topic_name] = {"data": area} + self.nav_area_tracker.update_area(area) + + def execution_ui_callback(self, topic_name, msg): + """Handle /roboflow/execution_ui messages.""" + state = msg.data.strip() + self.system_snapshot[topic_name] = {"data": state} + self.nav_area_tracker.update_execution_ui(state) + + def global_path_callback(self, topic_name, msg): + """Handle global-path (nav_msgs/msg/Path) messages.""" + self.path_deviation_tracker.update_path(msg) + self.system_snapshot[topic_name] = { + "num_poses": len(msg.poses), + } + + # ------------------------- + # Periodic Snapshot (timer_event) + # ------------------------- + def periodic_snapshot_callback(self): + """ + Periodically record all current data to MongoDB as a 'timer_event'. + + This does NOT increment incidents and is excluded from MDBI calculations. + """ + details = { + "system_snapshot": self.system_snapshot.copy(), + "metrics": self.get_metrics_snapshot(), + "navigation_area": self.nav_area_tracker.get_snapshot(), + "path_deviation": self.path_deviation_tracker.get_snapshot(), + "operation_mode": self.details.get("operation_mode"), + "estop": self.details.get("estop"), + } + self.log_event("timer_event", details) + self.get_logger().debug("[PeriodicSnapshot] timer_event logged to MongoDB") + def update_observed_mode_from_odom(self, is_moving: bool, now=None): """ Fallback mode inference when there is no explicit /gophar/operation_mode. @@ -993,6 +1067,10 @@ def timer_callback(self): self.speed = 0.0 self.publish_speed(0.0) + # Update nav-area tracker with zero speed so status reflects stopped state + is_auto = self.details.get('operation_mode') == self.AUTO + self.nav_area_tracker.update_speed(0.0, is_auto) + def update_db_metrics(self): # MBDI = autonomous_distance / incidents if self.incidents != 0: diff --git a/src/autonomy_metrics/autonomy_metrics/navigation_area_tracker.py b/src/autonomy_metrics/autonomy_metrics/navigation_area_tracker.py new file mode 100644 index 0000000..38720a6 --- /dev/null +++ b/src/autonomy_metrics/autonomy_metrics/navigation_area_tracker.py @@ -0,0 +1,216 @@ +#!/usr/bin/env python3 +""" +NavigationAreaTracker +Author: Ibrahim Hroob - JABASAI + +Tracks the robot's navigation area (INSIDE_POLYTUNNEL / OUTSIDE_POLYTUNNEL / +TRANSITION_INTO_POLYTUNNEL), derives per-area average speeds, time-in-current-mode, +and a high-level robot status string. + +Published topics +---------------- +- mdbi_logger/avg_speed_inside_polytunnel (Float32) +- mdbi_logger/avg_speed_outside_polytunnel (Float32) +- mdbi_logger/time_in_current_mode (Float32, seconds) +- mdbi_logger/robot_status (String) +- mdbi_logger/navigation_area (String) +""" + +from std_msgs.msg import Float32, String + + +class NavigationAreaTracker: + """Manages navigation-area state, per-area speed averages, and status derivation.""" + + # Area constants (match the enum on /robot_navigation_area) + INSIDE = "INSIDE_POLYTUNNEL" + OUTSIDE = "OUTSIDE_POLYTUNNEL" + TRANSITION = "TRANSITION_INTO_POLYTUNNEL" + + # Derived status labels + STATUS_IN_ROW = "In-row" + STATUS_HEADLAND = "Headland" + STATUS_ROW_ENTRY = "Row-entry" + STATUS_PAUSED_OBSTACLE = "Paused due to obstacle" + STATUS_HELD_BY_PICKER = "Held by picker" + STATUS_UNKNOWN = "Unknown" + + def __init__(self, node, *, paused_speed_threshold: float = 0.01): + """ + Parameters + ---------- + node : rclpy.node.Node + Parent ROS 2 node (used for publishers, clock, logging). + paused_speed_threshold : float + Speed (m/s) below which the robot is considered stopped. + """ + self._node = node + self._paused_speed_thr = paused_speed_threshold + + # Current state + self.current_area: str | None = None + self.execution_ui_state: str = "idle" # "idle" or "paused" + self.current_status: str = self.STATUS_UNKNOWN + + # Running speed averages per area + self._speed_sum = {self.INSIDE: 0.0, self.OUTSIDE: 0.0} + self._speed_count = {self.INSIDE: 0, self.OUTSIDE: 0} + + # Time-in-current-mode (resets on every area transition) + self._mode_start_time = node.get_clock().now() + + # ---- Publishers ---- + self.avg_speed_inside_pub = node.create_publisher( + Float32, "mdbi_logger/avg_speed_inside_polytunnel", 10 + ) + self.avg_speed_outside_pub = node.create_publisher( + Float32, "mdbi_logger/avg_speed_outside_polytunnel", 10 + ) + self.time_in_mode_pub = node.create_publisher( + Float32, "mdbi_logger/time_in_current_mode", 10 + ) + self.status_pub = node.create_publisher( + String, "mdbi_logger/robot_status", 10 + ) + self.nav_area_pub = node.create_publisher( + String, "mdbi_logger/navigation_area", 10 + ) + + node.get_logger().info("[NavAreaTracker] Initialised") + + # ------------------------------------------------------------------ + # Public update methods (called from the main node's callbacks) + # ------------------------------------------------------------------ + + def update_area(self, area_str: str) -> None: + """Handle a new /robot_navigation_area message.""" + area = area_str.strip() + if area != self.current_area: + self._node.get_logger().info( + f"[NavAreaTracker] Area changed: {self.current_area} -> {area}" + ) + self.current_area = area + # Reset time-in-current-mode on every area transition + self._mode_start_time = self._node.get_clock().now() + + # Republish on a logger-namespaced topic + msg = String() + msg.data = area + self.nav_area_pub.publish(msg) + + def update_execution_ui(self, state_str: str) -> None: + """Handle a new /roboflow/execution_ui message.""" + state = state_str.strip().lower() + if state != self.execution_ui_state: + self._node.get_logger().info( + f"[NavAreaTracker] Execution UI: {self.execution_ui_state} -> {state}" + ) + self.execution_ui_state = state + + def update_speed(self, speed: float, is_autonomous: bool) -> None: + """ + Feed the current speed (called from odom / timer). + + * Accumulates speed samples for per-area averages. + * Derives the high-level robot status. + * Publishes all metrics. + """ + # Accumulate speed for the current area + if self.current_area in (self.INSIDE, self.OUTSIDE): + self._speed_sum[self.current_area] += speed + self._speed_count[self.current_area] += 1 + + self._derive_status(speed, is_autonomous) + self._publish_all() + + # ------------------------------------------------------------------ + # Status derivation + # ------------------------------------------------------------------ + + def _derive_status(self, speed: float, is_autonomous: bool) -> None: + """ + Priority order: + 1. Held by picker (execution_ui == "paused") + 2. Paused due to obstacle (autonomous, speed ≈ 0, not held by picker) + 3. Row-entry (TRANSITION_INTO_POLYTUNNEL) + 4. In-row (INSIDE_POLYTUNNEL) + 5. Headland (OUTSIDE_POLYTUNNEL) + 6. Unknown + """ + if self.execution_ui_state == "paused": + self.current_status = self.STATUS_HELD_BY_PICKER + elif is_autonomous and speed < self._paused_speed_thr and self.current_area is not None: + self.current_status = self.STATUS_PAUSED_OBSTACLE + elif self.current_area == self.TRANSITION: + self.current_status = self.STATUS_ROW_ENTRY + elif self.current_area == self.INSIDE: + self.current_status = self.STATUS_IN_ROW + elif self.current_area == self.OUTSIDE: + self.current_status = self.STATUS_HEADLAND + else: + self.current_status = self.STATUS_UNKNOWN + + # ------------------------------------------------------------------ + # Publishing helpers + # ------------------------------------------------------------------ + + def _publish_all(self) -> None: + # Average speed INSIDE + msg = Float32() + msg.data = ( + float(self._speed_sum[self.INSIDE] / self._speed_count[self.INSIDE]) + if self._speed_count[self.INSIDE] > 0 + else 0.0 + ) + self.avg_speed_inside_pub.publish(msg) + + # Average speed OUTSIDE + msg = Float32() + msg.data = ( + float(self._speed_sum[self.OUTSIDE] / self._speed_count[self.OUTSIDE]) + if self._speed_count[self.OUTSIDE] > 0 + else 0.0 + ) + self.avg_speed_outside_pub.publish(msg) + + # Time in current mode + now = self._node.get_clock().now() + elapsed_sec = (now - self._mode_start_time).nanoseconds * 1e-9 + msg = Float32() + msg.data = float(elapsed_sec) + self.time_in_mode_pub.publish(msg) + + # Status string + msg = String() + msg.data = self.current_status + self.status_pub.publish(msg) + + # ------------------------------------------------------------------ + # Snapshot (for system_snapshot / MongoDB) + # ------------------------------------------------------------------ + + def get_snapshot(self) -> dict: + """Return the tracker state as a plain dict.""" + avg_in = ( + self._speed_sum[self.INSIDE] / self._speed_count[self.INSIDE] + if self._speed_count[self.INSIDE] > 0 + else 0.0 + ) + avg_out = ( + self._speed_sum[self.OUTSIDE] / self._speed_count[self.OUTSIDE] + if self._speed_count[self.OUTSIDE] > 0 + else 0.0 + ) + now = self._node.get_clock().now() + elapsed_sec = (now - self._mode_start_time).nanoseconds * 1e-9 + + return { + "navigation_area": self.current_area, + "execution_ui_state": self.execution_ui_state, + "robot_status": self.current_status, + "avg_speed_inside_polytunnel": avg_in, + "avg_speed_outside_polytunnel": avg_out, + "time_in_current_mode_sec": elapsed_sec, + "speed_samples_inside": self._speed_count[self.INSIDE], + "speed_samples_outside": self._speed_count[self.OUTSIDE], + } diff --git a/src/autonomy_metrics/autonomy_metrics/path_deviation_tracker.py b/src/autonomy_metrics/autonomy_metrics/path_deviation_tracker.py new file mode 100644 index 0000000..0cc7776 --- /dev/null +++ b/src/autonomy_metrics/autonomy_metrics/path_deviation_tracker.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +""" +PathDeviationTracker +Author: Ibrahim Hroob - JABASAI + +Subscribes (via the main node) to a global-path topic (nav_msgs/msg/Path) and +computes the cross-track error from the robot's current odometry position. + +Published topics +---------------- +- mdbi_logger/path_deviation_error (Float32, metres) +""" + +import math + +from std_msgs.msg import Float32 + + +class PathDeviationTracker: + """Computes the minimum distance from the robot to the global path.""" + + def __init__(self, node): + """ + Parameters + ---------- + node : rclpy.node.Node + Parent ROS 2 node (used for publishers, clock, logging). + """ + self._node = node + self._global_path: list[tuple[float, float]] = [] + self.deviation: float = 0.0 + + self.deviation_pub = node.create_publisher( + Float32, "mdbi_logger/path_deviation_error", 10 + ) + + node.get_logger().info("[PathDevTracker] Initialised") + + # ------------------------------------------------------------------ + # Public update methods + # ------------------------------------------------------------------ + + def update_path(self, path_msg) -> None: + """ + Called when a new nav_msgs/msg/Path arrives. + + Extracts the (x, y) waypoints and stores them for deviation + computation. + """ + self._global_path = [ + (pose.pose.position.x, pose.pose.position.y) + for pose in path_msg.poses + ] + self._node.get_logger().info( + f"[PathDevTracker] Received global path with " + f"{len(self._global_path)} poses" + ) + + def update_position(self, x: float, y: float) -> None: + """ + Called from the odometry callback with the robot's current position. + + Computes the minimum distance to any segment of the stored global path + and publishes the result. + """ + if not self._global_path or len(self._global_path) < 2: + return + + min_dist = float("inf") + for i in range(len(self._global_path) - 1): + d = self._point_to_segment_dist( + x, y, self._global_path[i], self._global_path[i + 1] + ) + if d < min_dist: + min_dist = d + + self.deviation = min_dist + + msg = Float32() + msg.data = float(self.deviation) + self.deviation_pub.publish(msg) + + # ------------------------------------------------------------------ + # Geometry helper + # ------------------------------------------------------------------ + + @staticmethod + def _point_to_segment_dist( + px: float, + py: float, + seg_a: tuple[float, float], + seg_b: tuple[float, float], + ) -> float: + """Minimum distance from point (px, py) to line segment (seg_a → seg_b).""" + ax, ay = seg_a + bx, by = seg_b + dx, dy = bx - ax, by - ay + len_sq = dx * dx + dy * dy + if len_sq == 0.0: + return math.sqrt((px - ax) ** 2 + (py - ay) ** 2) + t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / len_sq)) + proj_x = ax + t * dx + proj_y = ay + t * dy + return math.sqrt((px - proj_x) ** 2 + (py - proj_y) ** 2) + + # ------------------------------------------------------------------ + # Snapshot (for system_snapshot / MongoDB) + # ------------------------------------------------------------------ + + def get_snapshot(self) -> dict: + """Return the tracker state as a plain dict.""" + return { + "path_deviation_error": self.deviation, + "global_path_length": len(self._global_path), + } diff --git a/src/autonomy_metrics/config/metrics_full.yaml b/src/autonomy_metrics/config/metrics_full.yaml index ab97e67..4d7e719 100644 --- a/src/autonomy_metrics/config/metrics_full.yaml +++ b/src/autonomy_metrics/config/metrics_full.yaml @@ -129,4 +129,31 @@ topics: - name: "/gophar/motor_status/motor_1" type: "dynium_gophar_interfaces/msg/MotorStatus" - log_all_fields: true \ No newline at end of file + log_all_fields: true + + # --------------------------------------------------------- + # Navigation Area (INSIDE_POLYTUNNEL / OUTSIDE_POLYTUNNEL / TRANSITION_INTO_POLYTUNNEL) + # --------------------------------------------------------- + - name: "/robot_navigation_area" + type: "std_msgs/msg/String" + role: "navigation_area" + log_fields: + - "data" + + # --------------------------------------------------------- + # Execution UI (idle / paused) – "paused" means held by picker + # --------------------------------------------------------- + - name: "/roboflow/execution_ui" + type: "std_msgs/msg/String" + role: "execution_ui" + log_fields: + - "data" + + # --------------------------------------------------------- + # Global Path – used for path-deviation error computation + # Adjust the topic name to match your planner output. + # --------------------------------------------------------- + - name: "/plan" + type: "nav_msgs/msg/Path" + role: "global_path" + log_fields: [] \ No newline at end of file From 41f9cc0d388ba42688953dc660705dcd39683099 Mon Sep 17 00:00:00 2001 From: Ibrahim Date: Sun, 29 Mar 2026 17:58:16 +0100 Subject: [PATCH 3/3] Add autonomy metrics web UI and related functionality - Implemented a Flask-based web server for displaying autonomy metrics. - Created HTML, CSS, and JavaScript files for the dashboard interface. - Added utility functions for ROS message handling and serialization. - Configured YAML for metrics and topic definitions. - Developed a fake ROS topic stream for testing purposes. - Established MongoDB integration for session history and event logging. - Launched the web UI with customizable parameters for host and port. --- README.md | 793 ++----- .../autonomy_metrics/db_mgr.py | 196 +- .../autonomy_metrics/git_utils.py | 89 + .../autonomy_metrics/metric_logger.py | 1890 ++++++----------- .../navigation_area_tracker.py | 216 -- .../path_deviation_tracker.py | 115 - .../autonomy_metrics/topic_utils.py | 64 + src/autonomy_metrics/config/metrics_full.yaml | 181 +- src/autonomy_metrics/config/metrics_test.yaml | 62 + .../launch/autonomy_metrics.launch.py | 218 +- src/autonomy_metrics/package.xml | 11 +- src/autonomy_metrics/setup.py | 10 +- .../autonomy_metrics_webui/__init__.py | 1 + .../static/dashboard.css | 276 +++ .../static/dashboard.js | 163 ++ .../templates/index.html | 147 ++ .../autonomy_metrics_webui/webui_server.py | 265 +++ .../launch/autonomy_metrics_webui.launch.py | 41 + src/autonomy_metrics_webui/package.xml | 22 + .../resource/autonomy_metrics_webui | 1 + src/autonomy_metrics_webui/setup.cfg | 5 + src/autonomy_metrics_webui/setup.py | 30 + tools/fake_autonomy_metrics_stream.py | 188 ++ 23 files changed, 2402 insertions(+), 2582 deletions(-) create mode 100644 src/autonomy_metrics/autonomy_metrics/git_utils.py delete mode 100644 src/autonomy_metrics/autonomy_metrics/navigation_area_tracker.py delete mode 100644 src/autonomy_metrics/autonomy_metrics/path_deviation_tracker.py create mode 100644 src/autonomy_metrics/autonomy_metrics/topic_utils.py create mode 100644 src/autonomy_metrics/config/metrics_test.yaml create mode 100644 src/autonomy_metrics_webui/autonomy_metrics_webui/__init__.py create mode 100644 src/autonomy_metrics_webui/autonomy_metrics_webui/static/dashboard.css create mode 100644 src/autonomy_metrics_webui/autonomy_metrics_webui/static/dashboard.js create mode 100644 src/autonomy_metrics_webui/autonomy_metrics_webui/templates/index.html create mode 100644 src/autonomy_metrics_webui/autonomy_metrics_webui/webui_server.py create mode 100644 src/autonomy_metrics_webui/launch/autonomy_metrics_webui.launch.py create mode 100644 src/autonomy_metrics_webui/package.xml create mode 100644 src/autonomy_metrics_webui/resource/autonomy_metrics_webui create mode 100644 src/autonomy_metrics_webui/setup.cfg create mode 100644 src/autonomy_metrics_webui/setup.py create mode 100644 tools/fake_autonomy_metrics_stream.py diff --git a/README.md b/README.md index 216c9d3..2a4a4a4 100644 --- a/README.md +++ b/README.md @@ -1,701 +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). -- Tracks navigation area (in-row / headland / row-entry) with per-area average speed. -- Derives a high-level robot status (In-row, Headland, Row-entry, Paused due to obstacle, Held by picker). -- Computes cross-track deviation from the global path. -- Periodically logs a full system snapshot to MongoDB (`timer_event`, not counted as MDBI incident). - ---- - -## 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. Periodically logs a `timer_event` (default every 5 s) with the full system snapshot to MongoDB. These events are **not** counted as incidents and do **not** affect MDBI. -7. 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. | -| `snapshot_timer_interval` | double | `5.0` | Interval (s) between periodic `timer_event` snapshots logged to MongoDB. | - -### 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_moving_threshold` | double | `0.03` | `/cmd_vel/collision.linear.x` considered moving if abs(x) ≥ this (hysteresis upper bound). | -| `collision_zero_required_count` | int | `2` | Require this many consecutive near-zero collision samples before counting a collision. | -| `collision_time_window` | double | `0.5` | Nav and collision commands must both be within this time window (s). | -| `collision_log_cooldown` | double | `1.0` | Minimum time between logged collision events. | -| `intervention_event_cooldown` | double | `1.0` | Default cooldown for YAML `intervention_on_message` triggers. | - -Collision detection (robust falling-edge): - -* Keep last `/cmd_vel/nav` and `/cmd_vel/collision`. -* Compute a collision **only when**: - - * Current mode is `Autonomous`, - * Previous collision stream state was moving (`>= collision_moving_threshold`), - * Current collision stream is near-zero (`<= collision_zero_threshold`) for `collision_zero_required_count` consecutive samples, - * Latest `/cmd_vel/nav.linear.x` > `collision_nav_threshold`, - * Commands are recent (`≤ collision_time_window`). -* Additional safeguard: - - * Events are rate-limited by `collision_log_cooldown`. -* 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" - require_autonomous_mode: true - on_rising_edge: true - cooldown_sec: 1.0 - activity_field: "linear.x" - min_abs_value: 0.02 - - - 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: [] - - # Navigation area, execution UI, and global path - - name: "/robot_navigation_area" - type: "std_msgs/msg/String" - role: "navigation_area" - log_fields: - - "data" - - - name: "/roboflow/execution_ui" - type: "std_msgs/msg/String" - role: "execution_ui" - log_fields: - - "data" - - - name: "/plan" - type: "nav_msgs/msg/Path" - role: "global_path" - 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"`). +git_repos_base_path: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" -* `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. - * `"navigation_area"`: navigation area enum (e.g. `/robot_navigation_area`). - * `"execution_ui"`: execution UI state (`idle` / `paused`, e.g. `/roboflow/execution_ui`). - * `"global_path"`: global planner path (`nav_msgs/msg/Path`) used for path-deviation error. - -* `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`. +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`: - - * `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" - require_autonomous_mode: true - on_rising_edge: true - cooldown_sec: 1.0 - activity_field: "linear.x" - min_abs_value: 0.02 - ``` - - If enabled, events are emitted only when trigger conditions pass: - - * `require_autonomous_mode` (default `true`): skip in Manual mode. - * `on_rising_edge` (default `true`): trigger only on inactive→active transition. - * `cooldown_sec` (default from node param `intervention_event_cooldown`): per-trigger rate limit. - * `activity_field` (default `linear.x`): field used to determine activity. - * `min_abs_value`: minimum absolute value for numeric activity fields. - - This avoids noisy per-message logs and false overrides from zero/idle commands. - -* `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. Navigation Area, Status & Path Deviation +### Supported Roles -These features are implemented in two modular tracker classes that are instantiated by the main node. +- `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. -### Navigation area tracking (`NavigationAreaTracker`) +### Snapshot Logging -Subscribes (via `role: "navigation_area"`) to a topic publishing `std_msgs/msg/String` with one of: +Set `snapshot: true` on any configured topic you want included in the periodic MongoDB snapshot. The snapshot stores: -* `INSIDE_POLYTUNNEL` -* `OUTSIDE_POLYTUNNEL` -* `TRANSITION_INTO_POLYTUNNEL` +- the current computed summary +- the latest value seen for each configured snapshot topic +- a timestamp -Also monitors the execution UI topic (`role: "execution_ui"`) for values `idle` or `paused`. +## MongoDB Layout -#### Published topics +The rewritten logger stores data in: -| Topic | Type | Description | -| ------------------------------------------- | ------- | -------------------------------------------------------- | -| `mdbi_logger/avg_speed_inside_polytunnel` | Float32 | Running average speed while inside a polytunnel. | -| `mdbi_logger/avg_speed_outside_polytunnel` | Float32 | Running average speed while outside a polytunnel. | -| `mdbi_logger/time_in_current_mode` | Float32 | Seconds since the last area transition (resets on change).| -| `mdbi_logger/robot_status` | String | Derived high-level status (see below). | -| `mdbi_logger/navigation_area` | String | Current navigation area string. | +- `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 -#### Robot status derivation (priority order) +## Published ROS Topics -| Priority | Status | Condition | -| -------- | ----------------------- | ------------------------------------------------------------- | -| 1 | **Held by picker** | `/roboflow/execution_ui` is `"paused"` | -| 2 | **Paused due to obstacle** | Autonomous mode, speed ≈ 0, area is known | -| 3 | **Row-entry** | Area = `TRANSITION_INTO_POLYTUNNEL` | -| 4 | **In-row** | Area = `INSIDE_POLYTUNNEL` | -| 5 | **Headland** | Area = `OUTSIDE_POLYTUNNEL` | -| 6 | **Unknown** | No area data received yet | +- `/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` -### Path deviation tracking (`PathDeviationTracker`) +## Launching -Subscribes (via `role: "global_path"`) to a `nav_msgs/msg/Path` topic (default `/plan`). - -On every odometry update the tracker computes the minimum perpendicular distance from the robot's current position to the closest segment of the received global path. - -| Topic | Type | Description | -| --------------------------------- | ------- | ------------------------------------------ | -| `mdbi_logger/path_deviation_error`| Float32 | Cross-track error to the global path (m). | - -### Periodic snapshot (`timer_event`) - -A timer (configurable via `snapshot_timer_interval`, default 5 s) fires periodically and logs a `timer_event` to MongoDB. This event captures: - -* Full `system_snapshot` (latest values from all subscribed topics) -* Current metrics (distance, autonomous distance, speed, battery) -* Navigation area tracker snapshot (area, status, per-area avg speeds, time-in-mode) -* Path deviation tracker snapshot (error, path length) -* Current `operation_mode` and `estop` state - -**`timer_event` does NOT increment `incidents` and does NOT affect MDBI.** - ---- - -## 6. 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 suppressed by default for intervention triggers, - * Can still be explicitly logged for critical events (e.g. E-stop), - * Do **not** increment `incidents`, thus do not affect MDBI. - -### Recommended anti-noise presets - -Simulation (cleaner signals): - -* `mode_observer_speed_threshold: 0.01` -* `collision_nav_threshold: 0.01` -* `collision_zero_threshold: 0.001` -* `collision_moving_threshold: 0.02` -* `collision_zero_required_count: 2` -* `collision_log_cooldown: 0.6` -* `intervention_event_cooldown: 0.8` - -Real robot (noisier signals): - -* `mode_observer_speed_threshold: 0.02` -* `collision_nav_threshold: 0.03` -* `collision_zero_threshold: 0.01` -* `collision_moving_threshold: 0.05` -* `collision_zero_required_count: 3` -* `collision_log_cooldown: 1.5` -* `intervention_event_cooldown: 1.5` - ---- - -## 7. 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_moving_threshold': LaunchConfiguration('collision_moving_threshold'), - 'collision_zero_required_count': LaunchConfiguration('collision_zero_required_count'), - 'collision_time_window': LaunchConfiguration('collision_time_window'), - 'collision_log_cooldown': LaunchConfiguration('collision_log_cooldown'), - 'intervention_event_cooldown': LaunchConfiguration('intervention_event_cooldown'), - 'snapshot_timer_interval': LaunchConfiguration('snapshot_timer_interval'), - }], - ) - - return LaunchDescription([ - config_yaml_arg, - # ... other DeclareLaunchArgument ... - metrics_logger_node, - ]) -``` - -### Example launch usage - -Minimal: +Build the two packages: ```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 +colcon build --packages-select autonomy_metrics autonomy_metrics_webui ``` -With remote DB + tuned thresholds: +Launch the logger and the dashboard together: ```bash -ros2 launch autonomy_metrics_logger autonomy_metrics_logger.launch.py \ - config_yaml:=/path/to/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:=27017 \ + mongodb_port:=27018 \ enable_remote_logging:=true \ - remote_mongodb_host:=10.0.0.42 \ + remote_mongodb_host:=YOUR_CLOUD_MONGO_HOST \ 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 + use_webui:=true \ + ui_host:=0.0.0.0 \ + ui_port:=8081 ``` ---- - -## 8. Testing Tips - -* Verify topics: - - ```bash - ros2 topic list - ros2 topic info /gophar_vehicle_controller/odometry - ros2 topic info /gophar/system_status - ``` - -* Trigger joystick interventions: +If you only want the dashboard package: - ```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}}" - ``` - -* Publish a navigation area: - - ```bash - ros2 topic pub --once /robot_navigation_area std_msgs/msg/String "{data: 'INSIDE_POLYTUNNEL'}" - ``` - -* Simulate picker hold: +```bash +ros2 launch autonomy_metrics_webui autonomy_metrics_webui.launch.py \ + mongodb_host:=localhost \ + mongodb_port:=27018 \ + ui_port:=8081 +``` - ```bash - ros2 topic pub --once /roboflow/execution_ui std_msgs/msg/String "{data: 'paused'}" - ``` +## Web UI -* Verify derived topics: +The web UI serves: - ```bash - ros2 topic echo /mdbi_logger/robot_status - ros2 topic echo /mdbi_logger/avg_speed_inside_polytunnel - ros2 topic echo /mdbi_logger/path_deviation_error - ``` +- live summary from `/autonomy_metrics/session_summary_json` +- heartbeat freshness from `/autonomy_metrics/heartbeat` +- recent events and snapshots from MongoDB -* Check DB content (Mongo shell or Mongo client) in `robot_incidents.sessions`. - Look for events with `event_type: "timer_event"` for periodic snapshots. +Default URL: ---- +```text +http://0.0.0.0:8081 +``` -## 9. Architecture Overview +Open it from a browser on the host machine using the machine IP or loopback address. -The node is structured with modular tracker classes to keep concerns separated: +## Notes On MDBI -``` -AutonomyMetricsLogger (metric_logger.py) -├── NavigationAreaTracker (navigation_area_tracker.py) -│ ├── Area tracking (INSIDE / OUTSIDE / TRANSITION) -│ ├── Per-area average speed -│ ├── Time-in-current-mode -│ └── Robot status derivation -├── PathDeviationTracker (path_deviation_tracker.py) -│ ├── Global path storage -│ └── Cross-track error computation -├── DatabaseMgr (db_mgr.py) -│ └── MongoDB read/write -└── Periodic snapshot timer (timer_event → MongoDB) -``` +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 95f7da4..313e7d0 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -1,1301 +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 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 -from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr -from autonomy_metrics.navigation_area_tracker import NavigationAreaTracker -from autonomy_metrics.path_deviation_tracker import PathDeviationTracker - +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 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'") - - 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}") +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_moving_threshold', 0.03) # collision cmd abs(linear.x) >= this to consider as "has velocity" - self.declare_parameter('collision_zero_required_count', 2) # require N consecutive near-zero collision commands before logging - 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.declare_parameter('intervention_event_cooldown', 1.0) # default cooldown for YAML intervention triggers - self.declare_parameter('snapshot_timer_interval', 5.0) # seconds between periodic snapshot events - - 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_moving_threshold = self.get_parameter('collision_moving_threshold').get_parameter_value().double_value - self.collision_zero_required_count = self.get_parameter('collision_zero_required_count').get_parameter_value().integer_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.intervention_event_cooldown = self.get_parameter('intervention_event_cooldown').get_parameter_value().double_value - self.snapshot_timer_interval = self.get_parameter('snapshot_timer_interval').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}, moving_thr={self.collision_moving_threshold}, " - f"zero_required={self.collision_zero_required_count}, time_window={self.collision_time_window}, " - f"cooldown={self.collision_log_cooldown}" + 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.collision_zero_streak = 0 - - 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 = {} - self.prev_message_trigger_active = {} - self.last_intervention_event_time = {} - - # 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) - - # --- Modular trackers --- - self.nav_area_tracker = NavigationAreaTracker( - self, paused_speed_threshold=self.mode_observer_speed_threshold + self.heartbeat_period_sec = float( + self.runtime_cfg.get("heartbeat_period_sec", 1.0) ) - self.path_deviation_tracker = PathDeviationTracker(self) - - # Periodic snapshot timer (records all data to MongoDB without affecting MDBI) - self.snapshot_timer = self.create_timer( - self.snapshot_timer_interval, self.periodic_snapshot_callback + 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) - elif role == 'navigation_area': - cb = lambda msg, n=name: self.navigation_area_callback(n, msg) - elif role == 'execution_ui': - cb = lambda msg, n=name: self.execution_ui_callback(n, msg) - elif role == 'global_path': - cb = lambda msg, n=name: self.global_path_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_when_not_counted: 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 - elif not log_when_not_counted: - self.get_logger().debug( - f"[Intervention] Suppressed event '{event_type}' in mode={current_mode} " - "(not counted and log_when_not_counted=False)." - ) - return - self.get_logger().info( - f"[Intervention] type={event_type}, mode={current_mode}, " - f"counted={count_incident}, total_incidents={self.incidents}" + def _setup_databases(self): + self.db_managers = [] + + 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, msg, 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 - """ - now = self.get_clock().now() - - # 1) Trigger on message activity (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") - require_auto = msg_trig.get("require_autonomous_mode", True) - on_rising_edge = msg_trig.get("on_rising_edge", True) - cooldown_sec = float(msg_trig.get("cooldown_sec", self.intervention_event_cooldown)) - - activity_field = msg_trig.get("activity_field", "linear.x") - min_abs_cfg = msg_trig.get("min_abs_value", None) - if min_abs_cfg is None: - if activity_field.startswith("linear.") or activity_field.startswith("angular."): - min_abs_value = float(self.mode_observer_speed_threshold) - else: - min_abs_value = 0.0 - else: - min_abs_value = float(min_abs_cfg) - - activity_value = None - try: - activity_value = get_nested_field(msg, activity_field) - except Exception: - if "data" in data: - activity_value = data["data"] - - if isinstance(activity_value, bool): - is_active = activity_value - elif isinstance(activity_value, (int, float)): - is_active = abs(float(activity_value)) >= min_abs_value - elif activity_value is None: - # If we cannot evaluate activity, treat any message as activity. - is_active = True - else: - is_active = bool(activity_value) - - trig_key = (topic_name, evt_type) - prev_active = self.prev_message_trigger_active.get(trig_key, False) - self.prev_message_trigger_active[trig_key] = is_active - - current_mode = self.details.get('operation_mode', self.MAN) - if require_auto and current_mode != self.AUTO: - self.get_logger().debug( - f"[Trigger] Skip '{evt_type}' on '{topic_name}' because mode={current_mode}" - ) - else: - should_fire = is_active and (not on_rising_edge or not prev_active) - if should_fire: - last_event_time = self.last_intervention_event_time.get(trig_key) - if last_event_time is not None: - dt = (now - last_event_time).nanoseconds * 1e-9 - if dt < cooldown_sec: - should_fire = False - self.get_logger().debug( - f"[Trigger] Cooldown active for '{evt_type}' ({dt:.3f}s < {cooldown_sec:.3f}s)" - ) - - if should_fire: - self.last_intervention_event_time[trig_key] = now - self.get_logger().info( - f"[Trigger] message_activity on '{topic_name}' -> event '{evt_type}', " - f"value={activity_value}, active={is_active}" - ) - self.trigger_intervention( - evt_type, - extra={ - "topic": topic_name, - "activity_field": activity_field, - "activity_value": activity_value, - }, - ) - - # 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] - self.prev_field_values[key] = new - - # First sample establishes baseline; don't emit a change event on startup. - if prev is None: - continue - - changed = (prev != 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" + 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, ) - 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.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, ) - 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 - - # Always feed the current position to the path-deviation tracker - self.path_deviation_tracker.update_position(pos.x, pos.y) - - 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}" + 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)) ) - 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) - - # Feed speed to the navigation-area tracker - is_auto = self.details.get('operation_mode') == self.AUTO - self.nav_area_tracker.update_speed(self.speed, is_auto) - - # Include tracker snapshots in the system snapshot - self.system_snapshot['navigation_area'] = self.nav_area_tracker.get_snapshot() - self.system_snapshot['path_deviation'] = self.path_deviation_tracker.get_snapshot() - - 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', log_when_not_counted=True) - - 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, msg, 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}") - - # ------------------------- - # Navigation Area / Execution UI / Global Path callbacks - # ------------------------- - def navigation_area_callback(self, topic_name, msg): - """Handle /robot_navigation_area messages.""" - area = msg.data.strip() - self.system_snapshot[topic_name] = {"data": area} - self.nav_area_tracker.update_area(area) - - def execution_ui_callback(self, topic_name, msg): - """Handle /roboflow/execution_ui messages.""" - state = msg.data.strip() - self.system_snapshot[topic_name] = {"data": state} - self.nav_area_tracker.update_execution_ui(state) - - def global_path_callback(self, topic_name, msg): - """Handle global-path (nav_msgs/msg/Path) messages.""" - self.path_deviation_tracker.update_path(msg) - self.system_snapshot[topic_name] = { - "num_poses": len(msg.poses), - } - - # ------------------------- - # Periodic Snapshot (timer_event) - # ------------------------- - def periodic_snapshot_callback(self): - """ - Periodically record all current data to MongoDB as a 'timer_event'. - - This does NOT increment incidents and is excluded from MDBI calculations. - """ - details = { - "system_snapshot": self.system_snapshot.copy(), - "metrics": self.get_metrics_snapshot(), - "navigation_area": self.nav_area_tracker.get_snapshot(), - "path_deviation": self.path_deviation_tracker.get_snapshot(), - "operation_mode": self.details.get("operation_mode"), - "estop": self.details.get("estop"), - } - self.log_event("timer_event", details) - self.get_logger().debug("[PeriodicSnapshot] timer_event logged to MongoDB") - - 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 -> ... - """ - # Only count collisions while the robot is in autonomous mode. - if self.details.get('operation_mode') != self.AUTO: - collision_abs_vx = abs(coll_vx_now) - if collision_abs_vx >= self.collision_moving_threshold: - self.collision_prev_has_velocity = True - elif collision_abs_vx <= self.collision_zero_threshold: - self.collision_prev_has_velocity = False - self.collision_zero_streak = 0 - return - - # 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_abs_vx = abs(coll_vx_now) - if collision_abs_vx >= self.collision_moving_threshold: - self.collision_prev_has_velocity = True - elif collision_abs_vx <= self.collision_zero_threshold: - self.collision_prev_has_velocity = False - self.collision_zero_streak = 0 - 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_abs_vx = abs(coll_vx_now) - if collision_abs_vx >= self.collision_moving_threshold: - self.collision_prev_has_velocity = True - elif collision_abs_vx <= self.collision_zero_threshold: - self.collision_prev_has_velocity = False - self.collision_zero_streak = 0 + 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_abs_vx = abs(coll_vx_now) - if collision_abs_vx >= self.collision_moving_threshold: - self.collision_prev_has_velocity = True - elif collision_abs_vx <= self.collision_zero_threshold: - self.collision_prev_has_velocity = False - self.collision_zero_streak = 0 - return - - # Hysteresis-based state on /cmd_vel/collision - collision_abs_vx = abs(coll_vx_now) - collision_is_zero_now = collision_abs_vx <= self.collision_zero_threshold - collision_has_velocity_now = collision_abs_vx >= self.collision_moving_threshold - - self.get_logger().debug( - f"[Collision] prev_has_vel={self.collision_prev_has_velocity}, " - f"now_has_vel={collision_has_velocity_now}, now_is_zero={collision_is_zero_now}, " - f"coll_vx_now={coll_vx_now:.3f}, nav_vx={nav_vx:.3f}, zero_streak={self.collision_zero_streak}" + for index in range(len(self.current_path) - 1) ) - - if collision_has_velocity_now: - self.collision_prev_has_velocity = True - self.collision_zero_streak = 0 - return - - if not collision_is_zero_now: - # In hysteresis band; keep previous state and wait for clearer signal. - return - - self.collision_zero_streak += 1 - - # Falling edge with debounce: had velocity before, now zero for N consecutive samples. - if self.collision_prev_has_velocity and self.collision_zero_streak >= int(self.collision_zero_required_count): - if self.last_collision_logged_time is not None: - dt_coll = (now - self.last_collision_logged_time).nanoseconds * 1e-9 - if dt_coll < self.collision_log_cooldown: - self.get_logger().debug( - f"[Collision] Cooldown active ({dt_coll:.3f}s < {self.collision_log_cooldown:.3f}s); skip log." - ) - self.collision_prev_has_velocity = False - self.collision_zero_streak = 0 - return - - self.collision_incidents += 1 - self.last_collision_logged_time = now - 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}, zero_count={self.collision_zero_streak})" - ) - self.log_collision_event() - self.collision_prev_has_velocity = False - self.collision_zero_streak = 0 - - # ------------------------- - # 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 + ] + ) - # Update nav-area tracker with zero speed so status reflects stopped state - is_auto = self.details.get('operation_mode') == self.AUTO - self.nav_area_tracker.update_speed(0.0, is_auto) + 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() - def update_db_metrics(self): - # MBDI = autonomous_distance / incidents - if self.incidents != 0: - mdbi_val = float(self.autonomous_distance) / float(self.incidents) + 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: - # No incidents → use autonomous distance as-is (no division) - mdbi_val = float(self.autonomous_distance) + live_speed_mps = self.current_speed_mps - 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}" - ) - - 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): @@ -1303,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/navigation_area_tracker.py b/src/autonomy_metrics/autonomy_metrics/navigation_area_tracker.py deleted file mode 100644 index 38720a6..0000000 --- a/src/autonomy_metrics/autonomy_metrics/navigation_area_tracker.py +++ /dev/null @@ -1,216 +0,0 @@ -#!/usr/bin/env python3 -""" -NavigationAreaTracker -Author: Ibrahim Hroob - JABASAI - -Tracks the robot's navigation area (INSIDE_POLYTUNNEL / OUTSIDE_POLYTUNNEL / -TRANSITION_INTO_POLYTUNNEL), derives per-area average speeds, time-in-current-mode, -and a high-level robot status string. - -Published topics ----------------- -- mdbi_logger/avg_speed_inside_polytunnel (Float32) -- mdbi_logger/avg_speed_outside_polytunnel (Float32) -- mdbi_logger/time_in_current_mode (Float32, seconds) -- mdbi_logger/robot_status (String) -- mdbi_logger/navigation_area (String) -""" - -from std_msgs.msg import Float32, String - - -class NavigationAreaTracker: - """Manages navigation-area state, per-area speed averages, and status derivation.""" - - # Area constants (match the enum on /robot_navigation_area) - INSIDE = "INSIDE_POLYTUNNEL" - OUTSIDE = "OUTSIDE_POLYTUNNEL" - TRANSITION = "TRANSITION_INTO_POLYTUNNEL" - - # Derived status labels - STATUS_IN_ROW = "In-row" - STATUS_HEADLAND = "Headland" - STATUS_ROW_ENTRY = "Row-entry" - STATUS_PAUSED_OBSTACLE = "Paused due to obstacle" - STATUS_HELD_BY_PICKER = "Held by picker" - STATUS_UNKNOWN = "Unknown" - - def __init__(self, node, *, paused_speed_threshold: float = 0.01): - """ - Parameters - ---------- - node : rclpy.node.Node - Parent ROS 2 node (used for publishers, clock, logging). - paused_speed_threshold : float - Speed (m/s) below which the robot is considered stopped. - """ - self._node = node - self._paused_speed_thr = paused_speed_threshold - - # Current state - self.current_area: str | None = None - self.execution_ui_state: str = "idle" # "idle" or "paused" - self.current_status: str = self.STATUS_UNKNOWN - - # Running speed averages per area - self._speed_sum = {self.INSIDE: 0.0, self.OUTSIDE: 0.0} - self._speed_count = {self.INSIDE: 0, self.OUTSIDE: 0} - - # Time-in-current-mode (resets on every area transition) - self._mode_start_time = node.get_clock().now() - - # ---- Publishers ---- - self.avg_speed_inside_pub = node.create_publisher( - Float32, "mdbi_logger/avg_speed_inside_polytunnel", 10 - ) - self.avg_speed_outside_pub = node.create_publisher( - Float32, "mdbi_logger/avg_speed_outside_polytunnel", 10 - ) - self.time_in_mode_pub = node.create_publisher( - Float32, "mdbi_logger/time_in_current_mode", 10 - ) - self.status_pub = node.create_publisher( - String, "mdbi_logger/robot_status", 10 - ) - self.nav_area_pub = node.create_publisher( - String, "mdbi_logger/navigation_area", 10 - ) - - node.get_logger().info("[NavAreaTracker] Initialised") - - # ------------------------------------------------------------------ - # Public update methods (called from the main node's callbacks) - # ------------------------------------------------------------------ - - def update_area(self, area_str: str) -> None: - """Handle a new /robot_navigation_area message.""" - area = area_str.strip() - if area != self.current_area: - self._node.get_logger().info( - f"[NavAreaTracker] Area changed: {self.current_area} -> {area}" - ) - self.current_area = area - # Reset time-in-current-mode on every area transition - self._mode_start_time = self._node.get_clock().now() - - # Republish on a logger-namespaced topic - msg = String() - msg.data = area - self.nav_area_pub.publish(msg) - - def update_execution_ui(self, state_str: str) -> None: - """Handle a new /roboflow/execution_ui message.""" - state = state_str.strip().lower() - if state != self.execution_ui_state: - self._node.get_logger().info( - f"[NavAreaTracker] Execution UI: {self.execution_ui_state} -> {state}" - ) - self.execution_ui_state = state - - def update_speed(self, speed: float, is_autonomous: bool) -> None: - """ - Feed the current speed (called from odom / timer). - - * Accumulates speed samples for per-area averages. - * Derives the high-level robot status. - * Publishes all metrics. - """ - # Accumulate speed for the current area - if self.current_area in (self.INSIDE, self.OUTSIDE): - self._speed_sum[self.current_area] += speed - self._speed_count[self.current_area] += 1 - - self._derive_status(speed, is_autonomous) - self._publish_all() - - # ------------------------------------------------------------------ - # Status derivation - # ------------------------------------------------------------------ - - def _derive_status(self, speed: float, is_autonomous: bool) -> None: - """ - Priority order: - 1. Held by picker (execution_ui == "paused") - 2. Paused due to obstacle (autonomous, speed ≈ 0, not held by picker) - 3. Row-entry (TRANSITION_INTO_POLYTUNNEL) - 4. In-row (INSIDE_POLYTUNNEL) - 5. Headland (OUTSIDE_POLYTUNNEL) - 6. Unknown - """ - if self.execution_ui_state == "paused": - self.current_status = self.STATUS_HELD_BY_PICKER - elif is_autonomous and speed < self._paused_speed_thr and self.current_area is not None: - self.current_status = self.STATUS_PAUSED_OBSTACLE - elif self.current_area == self.TRANSITION: - self.current_status = self.STATUS_ROW_ENTRY - elif self.current_area == self.INSIDE: - self.current_status = self.STATUS_IN_ROW - elif self.current_area == self.OUTSIDE: - self.current_status = self.STATUS_HEADLAND - else: - self.current_status = self.STATUS_UNKNOWN - - # ------------------------------------------------------------------ - # Publishing helpers - # ------------------------------------------------------------------ - - def _publish_all(self) -> None: - # Average speed INSIDE - msg = Float32() - msg.data = ( - float(self._speed_sum[self.INSIDE] / self._speed_count[self.INSIDE]) - if self._speed_count[self.INSIDE] > 0 - else 0.0 - ) - self.avg_speed_inside_pub.publish(msg) - - # Average speed OUTSIDE - msg = Float32() - msg.data = ( - float(self._speed_sum[self.OUTSIDE] / self._speed_count[self.OUTSIDE]) - if self._speed_count[self.OUTSIDE] > 0 - else 0.0 - ) - self.avg_speed_outside_pub.publish(msg) - - # Time in current mode - now = self._node.get_clock().now() - elapsed_sec = (now - self._mode_start_time).nanoseconds * 1e-9 - msg = Float32() - msg.data = float(elapsed_sec) - self.time_in_mode_pub.publish(msg) - - # Status string - msg = String() - msg.data = self.current_status - self.status_pub.publish(msg) - - # ------------------------------------------------------------------ - # Snapshot (for system_snapshot / MongoDB) - # ------------------------------------------------------------------ - - def get_snapshot(self) -> dict: - """Return the tracker state as a plain dict.""" - avg_in = ( - self._speed_sum[self.INSIDE] / self._speed_count[self.INSIDE] - if self._speed_count[self.INSIDE] > 0 - else 0.0 - ) - avg_out = ( - self._speed_sum[self.OUTSIDE] / self._speed_count[self.OUTSIDE] - if self._speed_count[self.OUTSIDE] > 0 - else 0.0 - ) - now = self._node.get_clock().now() - elapsed_sec = (now - self._mode_start_time).nanoseconds * 1e-9 - - return { - "navigation_area": self.current_area, - "execution_ui_state": self.execution_ui_state, - "robot_status": self.current_status, - "avg_speed_inside_polytunnel": avg_in, - "avg_speed_outside_polytunnel": avg_out, - "time_in_current_mode_sec": elapsed_sec, - "speed_samples_inside": self._speed_count[self.INSIDE], - "speed_samples_outside": self._speed_count[self.OUTSIDE], - } diff --git a/src/autonomy_metrics/autonomy_metrics/path_deviation_tracker.py b/src/autonomy_metrics/autonomy_metrics/path_deviation_tracker.py deleted file mode 100644 index 0cc7776..0000000 --- a/src/autonomy_metrics/autonomy_metrics/path_deviation_tracker.py +++ /dev/null @@ -1,115 +0,0 @@ -#!/usr/bin/env python3 -""" -PathDeviationTracker -Author: Ibrahim Hroob - JABASAI - -Subscribes (via the main node) to a global-path topic (nav_msgs/msg/Path) and -computes the cross-track error from the robot's current odometry position. - -Published topics ----------------- -- mdbi_logger/path_deviation_error (Float32, metres) -""" - -import math - -from std_msgs.msg import Float32 - - -class PathDeviationTracker: - """Computes the minimum distance from the robot to the global path.""" - - def __init__(self, node): - """ - Parameters - ---------- - node : rclpy.node.Node - Parent ROS 2 node (used for publishers, clock, logging). - """ - self._node = node - self._global_path: list[tuple[float, float]] = [] - self.deviation: float = 0.0 - - self.deviation_pub = node.create_publisher( - Float32, "mdbi_logger/path_deviation_error", 10 - ) - - node.get_logger().info("[PathDevTracker] Initialised") - - # ------------------------------------------------------------------ - # Public update methods - # ------------------------------------------------------------------ - - def update_path(self, path_msg) -> None: - """ - Called when a new nav_msgs/msg/Path arrives. - - Extracts the (x, y) waypoints and stores them for deviation - computation. - """ - self._global_path = [ - (pose.pose.position.x, pose.pose.position.y) - for pose in path_msg.poses - ] - self._node.get_logger().info( - f"[PathDevTracker] Received global path with " - f"{len(self._global_path)} poses" - ) - - def update_position(self, x: float, y: float) -> None: - """ - Called from the odometry callback with the robot's current position. - - Computes the minimum distance to any segment of the stored global path - and publishes the result. - """ - if not self._global_path or len(self._global_path) < 2: - return - - min_dist = float("inf") - for i in range(len(self._global_path) - 1): - d = self._point_to_segment_dist( - x, y, self._global_path[i], self._global_path[i + 1] - ) - if d < min_dist: - min_dist = d - - self.deviation = min_dist - - msg = Float32() - msg.data = float(self.deviation) - self.deviation_pub.publish(msg) - - # ------------------------------------------------------------------ - # Geometry helper - # ------------------------------------------------------------------ - - @staticmethod - def _point_to_segment_dist( - px: float, - py: float, - seg_a: tuple[float, float], - seg_b: tuple[float, float], - ) -> float: - """Minimum distance from point (px, py) to line segment (seg_a → seg_b).""" - ax, ay = seg_a - bx, by = seg_b - dx, dy = bx - ax, by - ay - len_sq = dx * dx + dy * dy - if len_sq == 0.0: - return math.sqrt((px - ax) ** 2 + (py - ay) ** 2) - t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / len_sq)) - proj_x = ax + t * dx - proj_y = ay + t * dy - return math.sqrt((px - proj_x) ** 2 + (py - proj_y) ** 2) - - # ------------------------------------------------------------------ - # Snapshot (for system_snapshot / MongoDB) - # ------------------------------------------------------------------ - - def get_snapshot(self) -> dict: - """Return the tracker state as a plain dict.""" - return { - "path_deviation_error": self.deviation, - "global_path_length": len(self._global_path), - } 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 4d7e719..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,149 +12,61 @@ git_repos: ppl_gofar_platform: "robots/ppl_gofar_platform" topological_navigation: "contrib/topological_navigation" sentor: "contrib/sentor" - kiss-icp: "contrib/kiss-icp" + kiss_icp: "contrib/kiss-icp" + +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 topics: - # --------------------------------------------------------- - # GPS - # --------------------------------------------------------- - - name: "/gps_base/fix" - type: "sensor_msgs/msg/NavSatFix" - log_all_fields: true + - 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 - # --------------------------------------------------------- - # 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 - + role: "distance_odometry" + 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" - require_autonomous_mode: true - on_rising_edge: true - cooldown_sec: 1.0 - activity_field: "linear.x" - min_abs_value: 0.02 - - # --------------------------------------------------------- - # 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 - - # --------------------------------------------------------- - # Navigation Area (INSIDE_POLYTUNNEL / OUTSIDE_POLYTUNNEL / TRANSITION_INTO_POLYTUNNEL) - # --------------------------------------------------------- - name: "/robot_navigation_area" type: "std_msgs/msg/String" role: "navigation_area" - log_fields: - - "data" - - # --------------------------------------------------------- - # Execution UI (idle / paused) – "paused" means held by picker - # --------------------------------------------------------- - - name: "/roboflow/execution_ui" - type: "std_msgs/msg/String" - role: "execution_ui" - log_fields: - - "data" + snapshot: true - # --------------------------------------------------------- - # Global Path – used for path-deviation error computation - # Adjust the topic name to match your planner output. - # --------------------------------------------------------- - name: "/plan" type: "nav_msgs/msg/Path" role: "global_path" - log_fields: [] \ No newline at end of file + snapshot: true + + - 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()