From e74f3c4690f38771a1ad18752814ecf691c95ec0 Mon Sep 17 00:00:00 2001 From: ibrahim Date: Wed, 26 Nov 2025 19:00:01 +0000 Subject: [PATCH 1/9] WIP refactor --- .../autonomy_metrics/metric_logger.py | 848 +++++++++--------- src/autonomy_metrics/config/metrics_full.yaml | 41 + .../launch/autonomy_metrics.launch.py | 95 +- 3 files changed, 468 insertions(+), 516 deletions(-) create mode 100644 src/autonomy_metrics/config/metrics_full.yaml diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index 098a480..c27f943 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -1,99 +1,123 @@ - +#!/usr/bin/env python3 """ -Licensed under the Apache License, Version 2.0 (the "License"); -you may not use this file except in compliance with the License. -You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - -Unless required by applicable law or agreed to in writing, software -distributed under the License is distributed on an "AS IS" BASIS, -WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -See the License for the specific language governing permissions and -limitations under the License. - -Author: Ibrahim Hroob -Email: ibrahim.hroub7@gmail.com +YAML-driven AutonomyMetricsLogger +Author: Ibrahim Hroob (adapted) """ import os import math -import rclpy import subprocess -from rclpy.node import Node -from sensor_msgs.msg import NavSatFix -from std_msgs.msg import Float32, Bool, Int8, String -from nav_msgs.msg import Odometry -from topological_navigation_msgs.msg import ExecutePolicyModeGoal -from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr +import yaml from datetime import datetime, timezone +from importlib import import_module + +import rclpy +from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data -# Try to import HunterStatus and handle if it is unavailable -try: - from hunter_msgs.msg import HunterStatus - HUNTER_MSG_AVAILABLE = True -except ImportError: - HUNTER_MSG_AVAILABLE = False - print("Warning: hunter_msgs.HunterStatus is not available. Hunter status callbacks will be disabled.") - -# Try to import DogroothStatus and handle if it is unavailable -try: - from dogtooth_msgs.msg import DogtoothStatus - DOGTOOTH_MSG_AVAILABLE = True -except ImportError: - DOGTOOTH_MSG_AVAILABLE = False - print("Warning: hunter_msgs.HunterStatus is not available. Hunter status callbacks will be disabled.") +# keep a few explicit imports used by default publishers (std msgs) +from std_msgs.msg import Bool, Float32, Int8, String -class AutonomyMetricsLogger(Node): +# Optional: existing DB manager +from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr + +# Helper: dynamic message import +def import_msg_type(type_str: str): + """ + Accepts strings like 'sensor_msgs/msg/NavSatFix' and returns the message class. + """ + try: + parts = type_str.split('/') + if len(parts) != 3: + raise ValueError("Message type must be like 'pkg/msg/MessageName'") + pkg, submodule, cls_name = parts + 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}") + +# Helper: get nested attribute (supports attributes and list indices) +def get_nested_field(obj, path: str): """ - A class for logging robot events to calculate MDBI (Mean Distance Between Incidents). - - Attributes: - mdbi (float): Mean Distance Between Incidents. - distance (float): Traveled distance in meters. - db_mgr (DBMgr): Instance of DatabaseMgr for database operations. - gps_data (dict): GPS data of the robot. - estop (bool): Emergency stop status. - battery_level (float): Battery level of the robot. - operation_mode (str): Operation mode of the robot. - previous_x (float): Previous x-coordinate for distance calculation. - previous_y (float): Previous y-coordinate for distance calculation. - env_variables (dict): Environmental variables for the robot session. + path examples: + - 'pose.pose.position.x' + - 'status.status' + - 'position_covariance[0]' """ + try: + cur = obj + for part in path.split('.'): + # support index like field[0] + if '[' in part and part.endswith(']'): + name, idx = part[:-1].split('[') + cur = getattr(cur, name) + cur = cur[int(idx)] + else: + cur = getattr(cur, part) + return cur + except Exception as e: + raise AttributeError(f"Failed to get '{path}': {e}") +class AutonomyMetricsLogger(Node): def __init__(self): - super().__init__('mdbi_logger') - self.get_logger().info('Initializing logging node') - - # Define useful global variables - self.mongo_host = None - self.mongo_port = None - self.aoc_scenario_path = '' - self.aoc_navigation_path = '' - self.mdbi = None # Mean Distance Between Incidents + super().__init__('mdbi_logger_dynamic') + + # Basic state + self.get_logger().info("Starting YAML-driven AutonomyMetricsLogger") + + # Default parameters + self.declare_parameter('config_yaml', '') + self.declare_parameter('mongodb_host', 'localhost') + self.declare_parameter('mongodb_port', 27017) + self.declare_parameter('min_distance_threshold', 0.2) + + # Read parameters + 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.min_distance_threshold = self.get_parameter('min_distance_threshold').get_parameter_value().double_value + + self.get_logger().info(f"Config path: {self.config_path}") + self.get_logger().info(f"Mongo host: {self.mongo_host}, port: {self.mongo_port}") + self.get_logger().info(f"min_distance_threshold: {self.min_distance_threshold}") + + # Database manager + self.db_mgr = DBMgr(host=self.mongo_host, port=self.mongo_port) + + # Internal metrics state (keep similar to your original node) + self.mdbi = None 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.MAN} self.previous_x = None self.previous_y = None self.init_pose = True self.first_gps_fix_received = False - self.tasks_received_from_coordinator = 0 - self.distance = 0 # Total traveled distance in meters - self.autonomous_distance = 0 - self.autonomous_time = 0 # Total time spent in autonomous mode (in seconds) - self.autonomous_start_time = None # Time when autonomous mode started - self.min_distance_threshold = 0.2 #meters - self.AUTO = 'Autonomous' - self.MAN = 'Manual' self.prev_speed = 0.0 self.speed = 0.0 - # Init some variables - self.details = {} - self.details['estop'] = False - self.details['operation_mode'] = self.MAN + # dynamic structures + self.dynamic_subs = [] + self.dynamic_publishers = {} # keyed by topic_name -> (publisher, message_class, publish_field) + self.topic_cfg_map = {} # keyed by topic_name -> cfg dict - # Get robot name and other environmental variables (Group them into a dictionary) + # Default static publishers (distance/speed/incidents/heartbeat) + self.heartbeat_publisher = self.create_publisher(Bool, 'mdbi_logger/heartbeat', 10) + self.heartbeat_timer = self.create_timer(1.0, self.publish_heartbeat) + self.distance_publisher = self.create_publisher(Float32, 'mdbi_logger/total_traveled_distance', 10) + self.incidents_publisher = self.create_publisher(Int8, 'mdbi_logger/total_incidents', 10) + self.speed_publisher = self.create_publisher(Float32, 'mdbi_logger/robot_speed', 10) + self.battery_publisher = self.create_publisher(Float32, 'mdbi_logger/battery_level', 10) + + # load yaml config and create dynamic subs/pubs + self.load_and_setup_config() + + # initialize DB session env_variables = { 'robot_name': os.getenv('ROBOT_NAME', 'UNDEFINED'), 'farm_name': os.getenv('FARM_NAME', 'UNDEFINED'), @@ -101,428 +125,360 @@ def __init__(self): 'application': os.getenv('APPLICATION', 'UNDEFINED'), 'scenario_name': os.getenv('SCENARIO_NAME', 'UNDEFINED'), } - - # Declare and get the ROS parameters, including MongoDB host and port - self.declare_and_get_parameters() + # try to capture git info if provided in config + git_repos = [] + try: + git_cfg = self.config.get('git_repos', {}) + for label, p in git_cfg.items(): + git_repos.append({label: self.get_git_info(p)}) + except Exception: + pass - # Initialize DatabaseMgr with the retrieved port - self.db_mgr = DBMgr(host=self.mongo_host, port=self.mongo_port) + self.db_mgr.init_session(env_variables, git_repos) - # Get aoc git repos info + # ------------------------- + # Config loading & setup + # ------------------------- + def load_and_setup_config(self): + # Load YAML + if not self.config_path: + raise RuntimeError("Parameter 'config_yaml' must be set to a YAML file path") + + with open(self.config_path, 'r') as f: + self.config = yaml.safe_load(f) or {} + + topics = self.config.get('topics', []) + if not isinstance(topics, list): + raise RuntimeError("'topics' must be a list in YAML config") + + for item in topics: + name = item.get('name') + type_str = item.get('type') + if not name or not type_str: + self.get_logger().warning(f"Skipping invalid topic config: {item}") + continue + + # store cfg + self.topic_cfg_map[name] = item + + # create publisher if requested + pub_cfg = item.get('publish', {}) + if pub_cfg.get('enable', False): + try: + pub_msg_cls = import_msg_type(pub_cfg['type']) + pub_topic = pub_cfg['topic'] + publisher = self.create_publisher(pub_msg_cls, pub_topic, 10) + self.dynamic_publishers[name] = (publisher, pub_msg_cls, pub_cfg.get('field')) + except Exception as e: + self.get_logger().error(f"Failed to create publisher for {name}: {e}") + self.dynamic_publishers[name] = None + else: + self.dynamic_publishers[name] = None + + # create subscription + try: + msg_cls = import_msg_type(type_str) + except Exception as e: + self.get_logger().error(f"Failed to import msg type {type_str} for topic {name}: {e}") + continue + + # Determine if this topic has a special role + role = item.get('role', '').lower() + if role == 'odometry': + # special odometry callback to compute distances + sub_cb = lambda msg, tn=name: self.odom_role_callback(tn, msg) + elif role == 'control_mode': + sub_cb = lambda msg, tn=name: self.control_mode_role_callback(tn, msg) + elif role == 'estop': + sub_cb = lambda msg, tn=name: self.estop_role_callback(tn, msg) + else: + # generic callback + log_fields = item.get('log_fields', []) + sub_cb = lambda msg, tn=name, lf=log_fields: self.generic_callback(tn, msg, lf) + + self.create_subscription(msg_cls, name, sub_cb, qos_profile_sensor_data) + self.get_logger().info(f"Subscribed to {name} as {type_str} (role={role})") + + # ------------------------- + # Role-specific callbacks + # ------------------------- + def odom_role_callback(self, topic_name, msg): + # Expect msg is nav_msgs/msg/Odometry try: - self.get_logger().info("Retrieving AOC git repository information...") - aoc_scenario_git_info = self.get_git_info(self.aoc_scenario_path) + pos = msg.pose.pose.position except Exception as e: - self.get_logger().error(f"Error while logging git info: {e}") - aoc_scenario_git_info = None + self.get_logger().error(f"Odometry callback failed to extract position: {e}") + return - try: - self.get_logger().info("Retrieving AOC Navigation git repository information...") - aoc_navigation_git_info = self.get_git_info(self.aoc_navigation_path) - except Exception as e: - self.get_logger().error(f"Error while logging git info: {e}") - aoc_navigation_git_info = None + # Generic logging of configured fields + cfg = self.topic_cfg_map.get(topic_name, {}) + for f in cfg.get('log_fields', []): + try: + v = get_nested_field(msg, f) + # attach to details and log a DB event if requested + self.details.setdefault('odom_fields', {})[f] = v + except Exception: + self.get_logger().debug(f"Could not read odom field {f}") - git_repos = [] - git_repos.append({'aoc_scenario_git_info': aoc_scenario_git_info}) - git_repos.append({'aoc_navigation_git_info': aoc_navigation_git_info}) + # distance initialization + if self.init_pose: + self.init_pose = False + self.previous_x = pos.x + self.previous_y = pos.y + return - # Subscribe to the topics - self.create_subscriptions() + dx = pos.x - self.previous_x + dy = pos.y - self.previous_y + dist = math.sqrt(dx*dx + dy*dy) - # Initialize session in the database - self.db_mgr.init_session(env_variables, git_repos) + if dist < self.min_distance_threshold: + return - # Heartbeat publisher - self.heartbeat_publisher = self.create_publisher(Bool, 'mdbi_logger/heartbeat', 10) - self.heartbeat_timer = self.create_timer(1.0, self.publish_heartbeat) # Publish every 1 seconds + # compute speed (time based) + current_time = self.get_clock().now() + if hasattr(self, 'previous_time'): + time_diff = (current_time - self.previous_time).nanoseconds * 1e-9 + self.speed = dist / time_diff if time_diff > 0 else 0.0 + else: + self.speed = 0.0 + self.previous_time = current_time - # Add publishers for distance, incidents, and speed - self.distance_publisher = self.create_publisher(Float32, 'mdbi_logger/total_traveled_distance', 10) - self.incidents_publisher = self.create_publisher(Int8, 'mdbi_logger/total_incidents', 10) - self.speed_publisher = self.create_publisher(Float32, 'mdbi_logger/robot_speed', 10) - self.battery_publisher = self.create_publisher(Float32, 'mdbi_logger/battery_level', 10) + self.distance += dist + if self.details.get('operation_mode') == self.AUTO: + self.autonomous_distance += dist - def publish_heartbeat(self): - # Create and publish the heartbeat message - heartbeat_msg = Bool() - heartbeat_msg.data = True # Set to True to indicate the node is alive - self.heartbeat_publisher.publish(heartbeat_msg) - self.get_logger().debug('Heartbeat published') + self.previous_x = pos.x + self.previous_y = pos.y - if self.prev_speed != self.speed: - self.prev_speed = self.speed - else: - self.publish_speed(0.0) + # publish metrics + self.publish_distance(self.distance) + self.publish_speed(self.speed) - def declare_and_get_parameters(self): - # Declare MongoDB host and port as ROS parameters - self.declare_parameter('mongodb_host', 'localhost') - self.declare_parameter('mongodb_port', 27017) - # Retrieve the parameters - self.mongo_host = self.get_parameter('mongodb_host').get_parameter_value().string_value - self.mongo_port = self.get_parameter('mongodb_port').get_parameter_value().integer_value - - # Log MongoDB host and port - self.get_logger().info(f"MongoDB Host: {self.mongo_host}") - self.get_logger().info(f"MongoDB Port: {self.mongo_port}") + # optionally publish a configured publisher mirroring some field + pub_cfg = self.dynamic_publishers.get(topic_name) + if pub_cfg: + publisher, pub_msg_cls, pub_field = pub_cfg + if pub_field: + try: + v = get_nested_field(msg, pub_field) + out_msg = pub_msg_cls() + # Common case: std_msgs with .data + if hasattr(out_msg, 'data'): + out_msg.data = v + publisher.publish(out_msg) + else: + # naive assign if same attribute name + for attr in dir(out_msg): + if not attr.startswith('_'): + try: + setattr(out_msg, attr, v) + publisher.publish(out_msg) + break + except Exception: + continue + except Exception as e: + self.get_logger().debug(f"Publish transform failed for {topic_name}: {e}") + + def control_mode_role_callback(self, topic_name, msg): + # Expect control mode to be a message containing a field that maps to operation mode string or integer + cfg = self.topic_cfg_map.get(topic_name, {}) + mode_field = cfg.get('mode_field', 'data') # default to std_msgs/String .data + try: + val = get_nested_field(msg, mode_field) + except Exception as e: + self.get_logger().error(f"Control mode: failed to read field {mode_field}: {e}") + return - # Declare aoc repos path params - self.declare_parameter('aoc_scenario_path', '') - self.declare_parameter('aoc_navigation_path', '') - # Retrieve the parameters - self.aoc_scenario_path = self.get_parameter('aoc_scenario_path').get_parameter_value().string_value - self.aoc_navigation_path = self.get_parameter('aoc_navigation_path').get_parameter_value().string_value + # interpret val: try to map ints to modes if mapping exists + mapping = cfg.get('mode_mapping', {}) # e.g. {"3": "Manual", "1": "Autonomous", "0": "Autonomous"} + new_mode = None + if isinstance(val, (int, float)): + new_mode = mapping.get(str(int(val)), None) + elif isinstance(val, str): + new_mode = mapping.get(val, val) + else: + new_mode = str(val) - # Log aoc_scenario_path and aoc_navigation_path - self.get_logger().info(f"AOC Scenario Path: {self.aoc_scenario_path}") - self.get_logger().info(f"AOC Navigation Path: {self.aoc_navigation_path}") + if new_mode is None: + # fallback: if val is int 3 -> MAN else -> AUTO + if isinstance(val, (int, float)) and int(val) == 3: + new_mode = self.MAN + else: + new_mode = self.AUTO + # emulate previous control_mode_callback behaviour + current_time = datetime.now() + if new_mode != self.details.get('operation_mode'): + previous_mode = self.details.get('operation_mode') + self.details['operation_mode'] = new_mode + self.get_logger().info(f"operation_mode : {new_mode}") - self.declare_parameter('min_distance_threshold', 0.2) - self.min_distance_threshold = self.get_parameter('min_distance_threshold').get_parameter_value().double_value - self.get_logger().info(f"min_distance_threshold: {self.min_distance_threshold}") + if previous_mode == self.AUTO and new_mode == self.MAN: + if self.autonomous_start_time is not None: + elapsed_autonomous_time = (current_time - self.autonomous_start_time).total_seconds() + self.autonomous_time += elapsed_autonomous_time + self.details['autonomous_time'] = self.autonomous_time + self.autonomous_start_time = None - param_defaults = { - 'gps_topic': '/gps_base/fix', - 'odometry_topic': '/diff_drive_controller/odom', - 'battery_status_topic': '/diff_drive_controller/battery_status', - 'estop_status_topic': '/diff_drive_controller/estop_status', - 'hunter_status_topic': '/hunter_status', # Change this line - 'dogtooth_status_topic': '/dogtooth_status', # Change this line - 'actioned_by_coordinator_topic': '/topological_navigation/execute_policy_mode/goal' - } - self.params = {} - for param, default in param_defaults.items(): - self.declare_parameter(param, default) - self.params[param] = self.get_parameter(param).get_parameter_value().string_value - - for key, value in self.params.items(): - self.get_logger().info(f"{key.replace('_', ' ').title()}: {value}") - - def create_subscriptions(self): - self.create_subscription(Float32, self.params['battery_status_topic'], self.battery_level_callback, qos_profile=qos_profile_sensor_data) - self.create_subscription(Bool, self.params['estop_status_topic'], self.estop_sub_callback, qos_profile=qos_profile_sensor_data) - self.create_subscription(NavSatFix, self.params['gps_topic'], self.gps_fix_callback, qos_profile=qos_profile_sensor_data) - self.create_subscription(Odometry, self.params['odometry_topic'], self.gps_odom_callback, qos_profile=qos_profile_sensor_data) - self.create_subscription(ExecutePolicyModeGoal, self.params['actioned_by_coordinator_topic'], self.coordinator_callback, qos_profile=qos_profile_sensor_data) - # Only subscribe to hunter status if the message is available - if HUNTER_MSG_AVAILABLE: - self.create_subscription(HunterStatus, self.params['hunter_status_topic'], self.hunter_status_callback, qos_profile=qos_profile_sensor_data) - else: - self.get_logger().warn("HunterStatus message not available, skipping hunter status subscription.") - # Only subscribe to dogtooth status if the message is available - if DOGTOOTH_MSG_AVAILABLE: - self.create_subscription(DogtoothStatus, self.params['dogtooth_status_topic'], self.dogtooth_status_callback, qos_profile=qos_profile_sensor_data) - else: - self.get_logger().warn("DogtoothStatus message not available, skipping dogtooth status subscription.") + elif new_mode == self.AUTO: + self.autonomous_start_time = current_time - def get_git_info(self, repo_path="."): + if new_mode == self.MAN: + self.incidents += 1 + self.log_event('Manual_override', self.details) + elif new_mode == self.AUTO: + self.log_event('Autonomous', self.details) + + def estop_role_callback(self, topic_name, msg): + # default expect Bool-like .data + cfg = self.topic_cfg_map.get(topic_name, {}) + estop_field = cfg.get('field', 'data') try: - # Get current commit hash - commit_hash = subprocess.check_output( - ["git", "rev-parse", "HEAD"], cwd=repo_path - ).decode().strip() - - # Get current commit message - commit_message = subprocess.check_output( - ["git", "log", "-1", "--pretty=%B"], cwd=repo_path - ).decode().strip() - - # Get current commit author - commit_author = subprocess.check_output( - ["git", "log", "-1", "--pretty=%an"], cwd=repo_path - ).decode().strip() - - # Get current commit date - commit_date = subprocess.check_output( - ["git", "log", "-1", "--pretty=%ct"], cwd=repo_path - ).decode().strip() - commit_date = datetime.fromtimestamp(int(commit_date)).strftime('%Y-%m-%d %H:%M:%S') + v = get_nested_field(msg, estop_field) + except Exception as e: + self.get_logger().error(f"Estop: failed to read {estop_field}: {e}") + return - # Get current branch name - branch_name = subprocess.check_output( - ["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=repo_path - ).decode().strip() + if bool(v) != bool(self.details.get('estop', False)): + self.details['estop'] = bool(v) + self.get_logger().info(f"Estop status changed to: {self.details['estop']}") + if self.details['estop']: + self.incidents += 1 + self.log_event('EMS', self.details) - # Get remote URL + # ------------------------- + # Generic callback + # ------------------------- + def generic_callback(self, topic_name, msg, log_fields): + # Log configured fields + data = {} + for f in log_fields or []: try: - remote_url = subprocess.check_output( - ["git", "config", "--get", "remote.origin.url"], cwd=repo_path - ).decode().strip() - except subprocess.CalledProcessError: - remote_url = "No remote found" - - git_info = { - "build_version": commit_hash[:7], # Short hash of the latest commit - "commit_message": commit_message, # Commit message - "commit_author": commit_author, # Commit author - "commit_date": commit_date, # Commit date - "branch_name": branch_name, # Current branch name - "remote_url": remote_url # Remote URL + v = get_nested_field(msg, f) + data[f] = v + except Exception as e: + self.get_logger().debug(f"generic_callback: failed to read {f} from {topic_name}: {e}") + + if data: + event = { + 'time': datetime.now(tz=timezone.utc), + 'event_type': f"topic:{topic_name}", + 'details': data } + self.db_mgr.add_event(event) - return git_info - except subprocess.CalledProcessError: - return { - "build_version": "Invalid repository", - "commit_message": "", - "commit_author": "", - "commit_date": "", - "branch_name": "Unknown", - "remote_url": "Unknown" - } - - def log_event(self, msg='', details={}): + # Optional publisher configured for this topic + pub_cfg = self.dynamic_publishers.get(topic_name) + if pub_cfg: + publisher, pub_msg_cls, pub_field = pub_cfg + try: + val = get_nested_field(msg, pub_field) + out_msg = pub_msg_cls() + if hasattr(out_msg, 'data'): + out_msg.data = val + publisher.publish(out_msg) + else: + # attempt shallow assignment of first writable attribute + for attr in dir(out_msg): + if not attr.startswith('_'): + try: + setattr(out_msg, attr, val) + publisher.publish(out_msg) + break + except Exception: + continue + except Exception as e: + self.get_logger().debug(f"generic_callback publish failed for {topic_name}: {e}") + + # ------------------------- + # Logging & DB updates + # ------------------------- + def log_event(self, msg='', details=None): + if details is None: + details = {} event_time = datetime.now(tz=timezone.utc) - event = { - 'time': event_time, - 'event_type': msg, - 'details': details, - } + event = {'time': event_time, 'event_type': msg, 'details': details} self.db_mgr.add_event(event) - # Publish the total number of incidents + # publish incidents incidents_msg = Int8() incidents_msg.data = self.incidents self.incidents_publisher.publish(incidents_msg) - # Also update MDBI and other metrics + # update DB metric values self.db_mgr.update_distance(self.distance) self.db_mgr.update_incidents(self.incidents) self.db_mgr.update_autonomous_distance(self.autonomous_distance) - self.mdbi = float(self.distance) / float(self.incidents) if self.incidents != 0 else 0.0 self.db_mgr.update_mdbi(self.mdbi) - def battery_level_callback(self, msg): - self.details['battery_voltage'] = msg.data - battery_msg = Float32() - battery_msg.data = msg.data - self.battery_publisher.publish(battery_msg) - self.get_logger().debug(f"Battery level published: {msg.data}") - - def estop_sub_callback(self, msg): - if msg.data != self.details['estop']: - self.details['estop'] = msg.data - self.get_logger().info(f"Estop status changed to: {self.details['estop']}") - - if self.details['estop']: - self.incidents += 1 - self.get_logger().info(f"Incident count incremented to: {self.incidents}") - self.log_event('EMS', self.details) - - def control_mode_callback(self, msg): - current_time = datetime.now() - - if msg.data != self.details['operation_mode']: - previous_mode = self.details['operation_mode'] - self.details['operation_mode'] = msg.data - self.get_logger().info(f"operation_mode : {msg.data}") - - # If switching from AUTO to MANUAL, calculate elapsed time in AUTO mode - if previous_mode == self.AUTO and self.details['operation_mode'] == self.MAN: - if self.autonomous_start_time is not None: - elapsed_autonomous_time = (current_time - self.autonomous_start_time).total_seconds() - self.autonomous_time += elapsed_autonomous_time - self.details['autonomous_time'] = self.autonomous_time - self.get_logger().info(f"Autonomous mode elapsed time: {elapsed_autonomous_time} seconds") - self.autonomous_start_time = None # Reset the start time - - # If switching to AUTO mode, start timing - elif self.details['operation_mode'] == self.AUTO: - self.autonomous_start_time = current_time - self.get_logger().info(f"Autonomous mode started at: {self.autonomous_start_time}") - - # Log the event - if self.details['operation_mode'] == self.MAN: - self.incidents += 1 - self.log_event('Manual_override', self.details) - elif self.details['operation_mode'] == self.AUTO: - self.log_event('Autonomous', self.details) - - def coordinator_callback(self, msg): - edge_id = msg.route.edge_id - source = msg.route.source - self.tasks_received_from_coordinator += 1 - self.get_logger().info(f"Task count incremented to: {self.tasks_received_from_coordinator}") - coordinator = {'edge_id': edge_id, 'source': source, 'tasks_received_from_coordinator': self.tasks_received_from_coordinator} - self.details['coordinator'] = coordinator - self.log_event('Coordinator_task', self.details) - - def hunter_status_callback(self, msg): - # Serialize HunterStatus into a dictionary - serialized_data = { - 'header': { - 'stamp': { - 'sec': msg.header.stamp.sec, - 'nanosec': msg.header.stamp.nanosec, - }, - 'frame_id': msg.header.frame_id - }, - 'linear_velocity': msg.linear_velocity, - 'steering_angle': msg.steering_angle, - 'vehicle_state': msg.vehicle_state, - 'control_mode': msg.control_mode, - 'error_code': msg.error_code, - 'battery_voltage': msg.battery_voltage, - 'actuator_states': [ - { - 'motor_id': actuator.motor_id, - 'rpm': actuator.rpm, - 'current': actuator.current, - 'pulse_count': actuator.pulse_count, - 'driver_voltage': actuator.driver_voltage, - 'driver_temperature': actuator.driver_temperature, - 'motor_temperature': actuator.motor_temperature, - 'driver_state': actuator.driver_state - } for actuator in msg.actuator_states - ] - } - self.details['hunter_status'] = serialized_data - - battery = Float32() - battery.data = msg.battery_voltage - self.battery_level_callback(battery) - - control_mode = String() - if msg.control_mode == 3: #this is manual mode in hunter - control_mode.data = self.MAN - elif msg.control_mode == 1 or msg.control_mode == 0: #this is auto mode in hunter - control_mode.data = self.AUTO - self.control_mode_callback(control_mode) - - def dogtooth_status_callback(self, msg): - # Serialize HunterStatus into a dictionary - serialized_data = { - 'header': { - 'stamp': { - 'sec': msg.header.stamp.sec, - 'nanosec': msg.header.stamp.nanosec, - }, - 'frame_id': msg.header.frame_id - }, - 'linear_velocity': msg.linear_velocity, - 'angular_velocity': msg.angular_velocity, - 'vehicle_state': msg.vehicle_state, - 'control_mode': msg.control_mode, - 'error_code': msg.error_code, - 'battery_voltage': msg.battery_voltage, - } - self.details['dogtooth_status'] = serialized_data - - battery = Float32() - battery.data = msg.battery_voltage - self.battery_level_callback(battery) - - control_mode = String() - if msg.control_mode == 3: #this is manual mode in hunter - control_mode.data = self.MAN - elif msg.control_mode == 1 or msg.control_mode == 0: #this is auto mode in hunter - control_mode.data = self.AUTO - self.control_mode_callback(control_mode) - - def gps_fix_callback(self, msg): - gps_data = { - 'latitude': msg.latitude, - 'longitude': msg.longitude, - 'altitude': msg.altitude, - 'position_covariance': list(msg.position_covariance), - 'position_covariance_type': msg.position_covariance_type, - 'status': msg.status.status, - 'service': msg.status.service - } - - self.details['gps_data'] = gps_data - - if not self.first_gps_fix_received: - self.first_gps_fix_received = True - self.log_event('First_GNSS_msg', self.details) - - def gps_odom_callback(self, msg): - position = msg.pose.pose.position - - if self._initialize_pose(position): - return - - distance = self._calculate_distance(position) - - if self._is_significant_distance(distance): - self._update_distances(distance, position) - - def _initialize_pose(self, position): - if self.init_pose: - self.init_pose = False - self.previous_x = position.x - self.previous_y = position.y - return True - return False - - def _calculate_distance(self, position): - dx = position.x - self.previous_x - dy = position.y - self.previous_y - return math.sqrt(dx**2 + dy**2) - - def _is_significant_distance(self, distance): - return distance >= self.min_distance_threshold - - def _update_distances(self, distance, position): - current_time = self.get_clock().now() - - # Calculate time difference between the last update and now - if hasattr(self, 'previous_time'): - time_diff = (current_time - self.previous_time).nanoseconds * 1e-9 # Convert to seconds - self.speed = distance / time_diff if time_diff > 0 else 0.0 + # ------------------------- + # Heartbeat & metrics publishing + # ------------------------- + def publish_heartbeat(self): + hb = Bool() + hb.data = True + self.heartbeat_publisher.publish(hb) + # If speed unchanged, publish zero to indicate stop + if self.prev_speed != self.speed: + self.prev_speed = self.speed else: - self.speed = 0.0 - - # Update the previous time - self.previous_time = current_time - - # Update the total traveled distance - self.distance += distance - if self.details['operation_mode'] == self.AUTO: - self.autonomous_distance += distance - self.previous_x = position.x - self.previous_y = position.y - - # Publish the distance and speed - self.publish_distance(self.distance) - self.publish_speed(self.speed) + self.publish_speed(0.0) def publish_distance(self, distance): - # Publish total traveled distance - distance_msg = Float32() - distance_msg.data = distance - self.distance_publisher.publish(distance_msg) - + dmsg = Float32() + dmsg.data = float(distance) + self.distance_publisher.publish(dmsg) self.db_mgr.update_distance(self.distance) self.db_mgr.update_autonomous_distance(self.autonomous_distance) - - # Log the speed and distance - self.get_logger().debug(f"Traveled distance: {self.distance} meters") + self.get_logger().debug(f"Distance published: {self.distance}") def publish_speed(self, speed): - # Publish robot speed - speed_msg = Float32() - speed_msg.data = speed - self.speed_publisher.publish(speed_msg) - - - def _log_distance(self, distance): - self.get_logger().debug(f"distance: {distance}") - + smsg = Float32() + smsg.data = float(speed) + self.speed_publisher.publish(smsg) + # ------------------------- + # Git info helper (optional) + # ------------------------- + def get_git_info(self, repo_path="."): + try: + commit_hash = subprocess.check_output(["git", "rev-parse", "HEAD"], cwd=repo_path).decode().strip() + commit_message = subprocess.check_output(["git", "log", "-1", "--pretty=%B"], cwd=repo_path).decode().strip() + commit_author = subprocess.check_output(["git", "log", "-1", "--pretty=%an"], cwd=repo_path).decode().strip() + commit_date = subprocess.check_output(["git", "log", "-1", "--pretty=%ct"], cwd=repo_path).decode().strip() + commit_date = datetime.fromtimestamp(int(commit_date)).strftime('%Y-%m-%d %H:%M:%S') + branch_name = subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=repo_path).decode().strip() + try: + remote_url = subprocess.check_output(["git", "config", "--get", "remote.origin.url"], cwd=repo_path).decode().strip() + except subprocess.CalledProcessError: + remote_url = "No remote" + return { + "build_version": commit_hash[:7], + "commit_message": commit_message, + "commit_author": commit_author, + "commit_date": commit_date, + "branch_name": branch_name, + "remote_url": remote_url + } + except Exception: + return { + "build_version": "Invalid repository", + "commit_message": "", + "commit_author": "", + "commit_date": "", + "branch_name": "Unknown", + "remote_url": "Unknown" + } def main(args=None): rclpy.init(args=args) - - autonomy_metric_logger = AutonomyMetricsLogger() - - rclpy.spin(autonomy_metric_logger) - autonomy_metric_logger.destroy_node() - rclpy.shutdown() - + node = AutonomyMetricsLogger() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': main() diff --git a/src/autonomy_metrics/config/metrics_full.yaml b/src/autonomy_metrics/config/metrics_full.yaml new file mode 100644 index 0000000..b074bd6 --- /dev/null +++ b/src/autonomy_metrics/config/metrics_full.yaml @@ -0,0 +1,41 @@ +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" + log_fields: + - "fault_shutdown" + - "hv_on" + - "ignition_switch" + - "charger_interlock" + - "battery_percentage" + + publish: + enable: true + topic: "mdbi_logger/battery_level" + type: "std_msgs/msg/Float32" + field: "battery_percentage" + diff --git a/src/autonomy_metrics/launch/autonomy_metrics.launch.py b/src/autonomy_metrics/launch/autonomy_metrics.launch.py index 0844fdd..7bf4ec1 100644 --- a/src/autonomy_metrics/launch/autonomy_metrics.launch.py +++ b/src/autonomy_metrics/launch/autonomy_metrics.launch.py @@ -1,75 +1,30 @@ -# -*- coding: utf-8 -*- -#! /usr/bin/env python3 - import os from launch import LaunchDescription -from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - - gps_topic = LaunchConfiguration('gps_topic', default='/gps_base/fix') - odometry_topic = LaunchConfiguration('odometry_topic', default='/diff_drive_controller/odom') - battery_status = LaunchConfiguration('battery_status_topic', default='/diff_drive_controller/battery_status') - estop_status = LaunchConfiguration('estop_status_topic', default='/diff_drive_controller/estop_status') - hunter_status = LaunchConfiguration('hunter_status_topic', default='/hunter_status') - actioned_by_coordinator_topic = LaunchConfiguration('actioned_by_coordinator_topic', default='/topological_navigation/execute_policy_mode/goal') - - min_distance_threshold = LaunchConfiguration('min_distance_threshold', default='0.2') - - # Path to the scenario and navigation repos - aoc_scenario_path = LaunchConfiguration('aoc_scenario_path', default='/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario') - aoc_navigation_path = LaunchConfiguration('aoc_navigation_path', default='/home/ros/aoc_strawberry_scenario_ws/src/aoc_navigation') - - # New parameters for MongoDB host and port - mongodb_host = LaunchConfiguration('mongodb_host', default='localhost') - mongodb_port = LaunchConfiguration('mongodb_port', default=os.getenv('MONGOD_PORT', '27017')) - - return LaunchDescription([ - # Declare Launch Arguments for the existing parameters - DeclareLaunchArgument( - 'gps_topic', default_value=gps_topic, description='GPS topic for base fix'), - DeclareLaunchArgument( - 'odometry_topic', default_value=odometry_topic, description='A topic for odometry'), - DeclareLaunchArgument( - 'battery_status_topic', default_value=battery_status, description='Battery status topic'), - DeclareLaunchArgument( - 'estop_status_topic', default_value=estop_status, description='E-stop status topic'), - DeclareLaunchArgument( - 'hunter_status_topic', default_value=hunter_status, description='hunter status topic'), - DeclareLaunchArgument( - 'actioned_by_coordinator_topic', default_value=actioned_by_coordinator_topic, description=''), - - DeclareLaunchArgument( - 'min_distance_threshold', default_value=min_distance_threshold, description=''), - - DeclareLaunchArgument( - 'aoc_scenario_path', default_value=aoc_scenario_path, description='path to aoc scenario'), - DeclareLaunchArgument( - 'aoc_navigation_path', default_value=aoc_navigation_path, description='path to aoc navigation'), - - # Declare Launch Arguments for the new MongoDB parameters - DeclareLaunchArgument( - 'mongodb_host', default_value=mongodb_host, description='MongoDB host address'), - DeclareLaunchArgument( - 'mongodb_port', default_value=mongodb_port, description='MongoDB port'), - - # Node Configuration - Node( - package='autonomy_metrics', - executable='metric_logger', - output='screen', - parameters=[{ - 'gps_topic': gps_topic, - 'odometry_topic': odometry_topic, - 'battery_status_topic': battery_status, - 'estop_status_topic': estop_status, - 'hunter_status_topic': hunter_status, - 'aoc_scenario_path': aoc_scenario_path, - 'aoc_navigation_path': aoc_navigation_path, - 'mongodb_host': mongodb_host, - 'mongodb_port': mongodb_port, - }], - ) - ]) + 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 YAML configuration file for mdbi_logger' + ) + + node = Node( + package='autonomy_metrics', + executable='metric_logger', # ensure your setup/install maps executable correctly + name='mdbi_logger', + output='screen', + parameters=[{ + 'config_yaml': LaunchConfiguration('config_yaml'), + # optional override + 'mongodb_host': 'localhost', + 'mongodb_port': 27017, + }] + ) + + return LaunchDescription([config_yaml_arg, node]) From f11aeb8f9e4c2cf5f58b4a0587cea8b35f1182ba Mon Sep 17 00:00:00 2001 From: ibrahim Date: Sun, 30 Nov 2025 20:14:49 +0000 Subject: [PATCH 2/9] Refactor AutonomyMetricsLogger and update configuration - Improved error handling for database manager import. - Added timeout parameter for detecting robot stops. - Enhanced message import function to support additional formats. - Updated YAML configuration with a new control mode topic. - Changed default MongoDB port in launch file from 27017 to 27018. --- .../autonomy_metrics/metric_logger.py | 471 ++++++++---------- src/autonomy_metrics/config/metrics_full.yaml | 25 +- .../launch/autonomy_metrics.launch.py | 2 +- 3 files changed, 236 insertions(+), 262 deletions(-) diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index c27f943..0500d82 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -1,36 +1,57 @@ #!/usr/bin/env python3 """ YAML-driven AutonomyMetricsLogger -Author: Ibrahim Hroob (adapted) +Author: Ibrahim Hroob - JABASAI """ import os import math import subprocess import yaml +import time from datetime import datetime, timezone from importlib import import_module import rclpy from rclpy.node import Node from rclpy.qos import qos_profile_sensor_data +from rclpy.time import Time -# keep a few explicit imports used by default publishers (std msgs) +# Keep a few explicit imports used by default publishers (std msgs) from std_msgs.msg import Bool, Float32, Int8, String # Optional: existing DB manager -from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr +try: + from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr +except ImportError: + # Fallback mock class if the module is missing, to prevent immediate crash during testing + class DBMgr: + def __init__(self, host, port): print(f"DB Mock connected to {host}:{port}") + def init_session(self, env, git): print("DB Session Init") + def add_event(self, evt): pass + def update_distance(self, d): pass + def update_incidents(self, i): pass + def update_autonomous_distance(self, d): pass + def update_mdbi(self, m): pass # Helper: dynamic message import def import_msg_type(type_str: str): """ - Accepts strings like 'sensor_msgs/msg/NavSatFix' and returns the message class. + Accepts strings like 'sensor_msgs/msg/NavSatFix' or 'std_msgs/String' + and returns the message class. """ try: parts = type_str.split('/') - if len(parts) != 3: - raise ValueError("Message type must be like 'pkg/msg/MessageName'") - pkg, submodule, cls_name = parts + + # Handle "pkg/Message" (2 parts) -> convert to "pkg/msg/Message" + 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: @@ -38,16 +59,9 @@ def import_msg_type(type_str: str): # Helper: get nested attribute (supports attributes and list indices) def get_nested_field(obj, path: str): - """ - path examples: - - 'pose.pose.position.x' - - 'status.status' - - 'position_covariance[0]' - """ try: cur = obj for part in path.split('.'): - # support index like field[0] if '[' in part and part.endswith(']'): name, idx = part[:-1].split('[') cur = getattr(cur, name) @@ -62,7 +76,6 @@ class AutonomyMetricsLogger(Node): def __init__(self): super().__init__('mdbi_logger_dynamic') - # Basic state self.get_logger().info("Starting YAML-driven AutonomyMetricsLogger") # Default parameters @@ -70,22 +83,23 @@ def __init__(self): self.declare_parameter('mongodb_host', 'localhost') self.declare_parameter('mongodb_port', 27017) self.declare_parameter('min_distance_threshold', 0.2) + # Timeout to consider robot stopped if no distance update occurs + self.declare_parameter('stop_timeout', 2.0) # Read parameters 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.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"Mongo host: {self.mongo_host}, port: {self.mongo_port}") - self.get_logger().info(f"min_distance_threshold: {self.min_distance_threshold}") - # Database manager self.db_mgr = DBMgr(host=self.mongo_host, port=self.mongo_port) - # Internal metrics state (keep similar to your original node) - self.mdbi = None + # Internal metrics state + self.mdbi = 0.0 self.incidents = 0 self.distance = 0.0 self.autonomous_distance = 0.0 @@ -94,30 +108,31 @@ def __init__(self): self.AUTO = 'Autonomous' self.MAN = 'Manual' self.details = {'estop': False, 'operation_mode': self.MAN} + + # Odometry / Speed state self.previous_x = None self.previous_y = None self.init_pose = True - self.first_gps_fix_received = False - self.prev_speed = 0.0 self.speed = 0.0 + self.last_odom_update_time = self.get_clock().now() # To detect stops - # dynamic structures self.dynamic_subs = [] - self.dynamic_publishers = {} # keyed by topic_name -> (publisher, message_class, publish_field) - self.topic_cfg_map = {} # keyed by topic_name -> cfg dict + self.dynamic_publishers = {} + self.topic_cfg_map = {} - # Default static publishers (distance/speed/incidents/heartbeat) + # Static Publishers self.heartbeat_publisher = self.create_publisher(Bool, 'mdbi_logger/heartbeat', 10) - self.heartbeat_timer = self.create_timer(1.0, self.publish_heartbeat) self.distance_publisher = self.create_publisher(Float32, 'mdbi_logger/total_traveled_distance', 10) self.incidents_publisher = self.create_publisher(Int8, 'mdbi_logger/total_incidents', 10) self.speed_publisher = self.create_publisher(Float32, 'mdbi_logger/robot_speed', 10) self.battery_publisher = self.create_publisher(Float32, 'mdbi_logger/battery_level', 10) + + # Timer + self.heartbeat_timer = self.create_timer(1.0, self.timer_callback) - # load yaml config and create dynamic subs/pubs self.load_and_setup_config() - # initialize DB session + # Init Session env_variables = { 'robot_name': os.getenv('ROBOT_NAME', 'UNDEFINED'), 'farm_name': os.getenv('FARM_NAME', 'UNDEFINED'), @@ -125,360 +140,296 @@ def __init__(self): 'application': os.getenv('APPLICATION', 'UNDEFINED'), 'scenario_name': os.getenv('SCENARIO_NAME', 'UNDEFINED'), } - # try to capture git info if provided in config + git_repos = [] - try: - git_cfg = self.config.get('git_repos', {}) - for label, p in git_cfg.items(): - git_repos.append({label: self.get_git_info(p)}) - except Exception: - pass + if self.config: + try: + git_cfg = self.config.get('git_repos', {}) + for label, p in git_cfg.items(): + git_repos.append({label: self.get_git_info(p)}) + except Exception: + pass self.db_mgr.init_session(env_variables, git_repos) - # ------------------------- - # Config loading & setup - # ------------------------- def load_and_setup_config(self): - # Load YAML if not self.config_path: - raise RuntimeError("Parameter 'config_yaml' must be set to a YAML file path") + self.get_logger().error("No 'config_yaml' parameter provided.") + self.config = {} + return - with open(self.config_path, 'r') as f: - self.config = yaml.safe_load(f) or {} + try: + with open(self.config_path, 'r') as f: + self.config = yaml.safe_load(f) or {} + except Exception as e: + self.get_logger().error(f"Failed to load YAML: {e}") + self.config = {} + return topics = self.config.get('topics', []) - if not isinstance(topics, list): - raise RuntimeError("'topics' must be a list in YAML config") - for item in topics: name = item.get('name') type_str = item.get('type') + if not name or not type_str: - self.get_logger().warning(f"Skipping invalid topic config: {item}") continue - # store cfg self.topic_cfg_map[name] = item - # create publisher if requested + # Create dynamic publisher if requested pub_cfg = item.get('publish', {}) if pub_cfg.get('enable', False): try: pub_msg_cls = import_msg_type(pub_cfg['type']) - pub_topic = pub_cfg['topic'] - publisher = self.create_publisher(pub_msg_cls, pub_topic, 10) - self.dynamic_publishers[name] = (publisher, pub_msg_cls, pub_cfg.get('field')) + pub = self.create_publisher(pub_msg_cls, pub_cfg['topic'], 10) + self.dynamic_publishers[name] = (pub, pub_msg_cls, pub_cfg.get('field')) except Exception as e: - self.get_logger().error(f"Failed to create publisher for {name}: {e}") + self.get_logger().error(f"Pub creation failed for {name}: {e}") self.dynamic_publishers[name] = None else: self.dynamic_publishers[name] = None - # create subscription + # Create subscription try: msg_cls = import_msg_type(type_str) - except Exception as e: - self.get_logger().error(f"Failed to import msg type {type_str} for topic {name}: {e}") - continue + role = item.get('role', '').lower() + + # Define callback with default args to capture loop variables + 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) + else: + lf = item.get('log_fields', []) + cb = lambda msg, n=name, l=lf: self.generic_callback(n, msg, l) - # Determine if this topic has a special role - role = item.get('role', '').lower() - if role == 'odometry': - # special odometry callback to compute distances - sub_cb = lambda msg, tn=name: self.odom_role_callback(tn, msg) - elif role == 'control_mode': - sub_cb = lambda msg, tn=name: self.control_mode_role_callback(tn, msg) - elif role == 'estop': - sub_cb = lambda msg, tn=name: self.estop_role_callback(tn, msg) - else: - # generic callback - log_fields = item.get('log_fields', []) - sub_cb = lambda msg, tn=name, lf=log_fields: self.generic_callback(tn, msg, lf) + self.create_subscription(msg_cls, name, cb, qos_profile_sensor_data) + self.get_logger().info(f"Subscribed to {name} ({role})") - self.create_subscription(msg_cls, name, sub_cb, qos_profile_sensor_data) - self.get_logger().info(f"Subscribed to {name} as {type_str} (role={role})") + except Exception as e: + self.get_logger().error(f"Sub creation failed for {name}: {e}") # ------------------------- - # Role-specific callbacks + # Callbacks # ------------------------- def odom_role_callback(self, topic_name, msg): - # Expect msg is nav_msgs/msg/Odometry try: pos = msg.pose.pose.position - except Exception as e: - self.get_logger().error(f"Odometry callback failed to extract position: {e}") + except Exception: return - # Generic logging of configured fields - cfg = self.topic_cfg_map.get(topic_name, {}) - for f in cfg.get('log_fields', []): - try: - v = get_nested_field(msg, f) - # attach to details and log a DB event if requested - self.details.setdefault('odom_fields', {})[f] = v - except Exception: - self.get_logger().debug(f"Could not read odom field {f}") + current_time = self.get_clock().now() - # distance initialization + # Initialization 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 return dx = pos.x - self.previous_x dy = pos.y - self.previous_y dist = math.sqrt(dx*dx + dy*dy) + # Distance accumulation threshold if dist < self.min_distance_threshold: + # We haven't moved enough to register distance, but we might check speed? + # Actually, standard logic is to assume stationary if under threshold + # We update the time so the heartbeat knows we are still receiving msgs + # but we don't update 'self.speed' here because division by small time is noisy. return - # compute speed (time based) - current_time = self.get_clock().now() - if hasattr(self, 'previous_time'): - time_diff = (current_time - self.previous_time).nanoseconds * 1e-9 - self.speed = dist / time_diff if time_diff > 0 else 0.0 - else: - self.speed = 0.0 - self.previous_time = current_time - + # Calculate time delta for speed + # Ensure we have a previous time set + if not hasattr(self, 'previous_time'): + self.previous_time = current_time + + time_diff = (current_time - self.previous_time).nanoseconds * 1e-9 + + # Update metrics + self.speed = dist / time_diff if time_diff > 0 else 0.0 self.distance += dist if self.details.get('operation_mode') == self.AUTO: self.autonomous_distance += dist + # Update state self.previous_x = pos.x self.previous_y = pos.y + self.previous_time = current_time + self.last_odom_update_time = current_time # Mark activity - # publish metrics + # Publish immediately on change self.publish_distance(self.distance) self.publish_speed(self.speed) - # optionally publish a configured publisher mirroring some field - pub_cfg = self.dynamic_publishers.get(topic_name) - if pub_cfg: - publisher, pub_msg_cls, pub_field = pub_cfg - if pub_field: - try: - v = get_nested_field(msg, pub_field) - out_msg = pub_msg_cls() - # Common case: std_msgs with .data - if hasattr(out_msg, 'data'): - out_msg.data = v - publisher.publish(out_msg) - else: - # naive assign if same attribute name - for attr in dir(out_msg): - if not attr.startswith('_'): - try: - setattr(out_msg, attr, v) - publisher.publish(out_msg) - break - except Exception: - continue - except Exception as e: - self.get_logger().debug(f"Publish transform failed for {topic_name}: {e}") + # Handle dynamic publish mirroring + self.handle_dynamic_publish(topic_name, msg) def control_mode_role_callback(self, topic_name, msg): - # Expect control mode to be a message containing a field that maps to operation mode string or integer cfg = self.topic_cfg_map.get(topic_name, {}) - mode_field = cfg.get('mode_field', 'data') # default to std_msgs/String .data + mode_field = cfg.get('mode_field', 'data') try: val = get_nested_field(msg, mode_field) - except Exception as e: - self.get_logger().error(f"Control mode: failed to read field {mode_field}: {e}") + except Exception: return - # interpret val: try to map ints to modes if mapping exists - mapping = cfg.get('mode_mapping', {}) # e.g. {"3": "Manual", "1": "Autonomous", "0": "Autonomous"} - new_mode = None - if isinstance(val, (int, float)): - new_mode = mapping.get(str(int(val)), None) - elif isinstance(val, str): - new_mode = mapping.get(val, val) - else: - new_mode = str(val) - + mapping = cfg.get('mode_mapping', {}) + # Robust lookup (string or int) + new_mode = mapping.get(str(val)) or mapping.get(val) + if new_mode is None: - # fallback: if val is int 3 -> MAN else -> AUTO - if isinstance(val, (int, float)) and int(val) == 3: - new_mode = self.MAN - else: - new_mode = self.AUTO + # Fallback logic + if str(val) == "3": new_mode = self.MAN + else: new_mode = self.AUTO - # emulate previous control_mode_callback behaviour current_time = datetime.now() + if new_mode != self.details.get('operation_mode'): - previous_mode = self.details.get('operation_mode') + prev_mode = self.details.get('operation_mode') self.details['operation_mode'] = new_mode - self.get_logger().info(f"operation_mode : {new_mode}") + self.get_logger().info(f"Mode changed: {prev_mode} -> {new_mode}") - if previous_mode == self.AUTO and new_mode == self.MAN: - if self.autonomous_start_time is not None: - elapsed_autonomous_time = (current_time - self.autonomous_start_time).total_seconds() - self.autonomous_time += elapsed_autonomous_time - self.details['autonomous_time'] = self.autonomous_time + # Time accumulation logic + 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 - elif new_mode == self.AUTO: self.autonomous_start_time = current_time + # Log events if new_mode == self.MAN: self.incidents += 1 self.log_event('Manual_override', self.details) - elif new_mode == self.AUTO: + else: self.log_event('Autonomous', self.details) def estop_role_callback(self, topic_name, msg): - # default expect Bool-like .data cfg = self.topic_cfg_map.get(topic_name, {}) - estop_field = cfg.get('field', 'data') + field = cfg.get('field', 'data') try: - v = get_nested_field(msg, estop_field) - except Exception as e: - self.get_logger().error(f"Estop: failed to read {estop_field}: {e}") + v = bool(get_nested_field(msg, field)) + except Exception: return - if bool(v) != bool(self.details.get('estop', False)): - self.details['estop'] = bool(v) - self.get_logger().info(f"Estop status changed to: {self.details['estop']}") - if self.details['estop']: + if v != self.details.get('estop', False): + self.details['estop'] = v + self.get_logger().info(f"E-Stop: {v}") + if v: self.incidents += 1 self.log_event('EMS', self.details) - # ------------------------- - # Generic callback - # ------------------------- def generic_callback(self, topic_name, msg, log_fields): - # Log configured fields data = {} - for f in log_fields or []: + for f in log_fields: try: - v = get_nested_field(msg, f) - data[f] = v - except Exception as e: - self.get_logger().debug(f"generic_callback: failed to read {f} from {topic_name}: {e}") - + data[f] = get_nested_field(msg, f) + except Exception: + pass + if data: - event = { + self.db_mgr.add_event({ 'time': datetime.now(tz=timezone.utc), 'event_type': f"topic:{topic_name}", 'details': data - } - self.db_mgr.add_event(event) + }) + + self.handle_dynamic_publish(topic_name, msg) - # Optional publisher configured for this topic + def handle_dynamic_publish(self, topic_name, msg): pub_cfg = self.dynamic_publishers.get(topic_name) - if pub_cfg: - publisher, pub_msg_cls, pub_field = pub_cfg - try: - val = get_nested_field(msg, pub_field) - out_msg = pub_msg_cls() - if hasattr(out_msg, 'data'): - out_msg.data = val - publisher.publish(out_msg) - else: - # attempt shallow assignment of first writable attribute - for attr in dir(out_msg): - if not attr.startswith('_'): - try: - setattr(out_msg, attr, val) - publisher.publish(out_msg) - break - except Exception: - continue - except Exception as e: - self.get_logger().debug(f"generic_callback publish failed for {topic_name}: {e}") - - # ------------------------- - # Logging & DB updates - # ------------------------- - def log_event(self, msg='', details=None): - if details is None: - details = {} - event_time = datetime.now(tz=timezone.utc) - event = {'time': event_time, 'event_type': msg, 'details': details} - self.db_mgr.add_event(event) - - # publish incidents - incidents_msg = Int8() - incidents_msg.data = self.incidents - self.incidents_publisher.publish(incidents_msg) - - # update DB metric values - self.db_mgr.update_distance(self.distance) - self.db_mgr.update_incidents(self.incidents) - self.db_mgr.update_autonomous_distance(self.autonomous_distance) - self.mdbi = float(self.distance) / float(self.incidents) if self.incidents != 0 else 0.0 - self.db_mgr.update_mdbi(self.mdbi) + 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'): + 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: continue + publisher.publish(out_msg) + except Exception: + pass # ------------------------- - # Heartbeat & metrics publishing + # Timer & Logging # ------------------------- - def publish_heartbeat(self): + def timer_callback(self): + # 1. Heartbeat hb = Bool() hb.data = True self.heartbeat_publisher.publish(hb) - # If speed unchanged, publish zero to indicate stop - if self.prev_speed != self.speed: - self.prev_speed = self.speed - else: + + # 2. Check for robot stop (TIMEOUT LOGIC) + # If we haven't processed a distance update in X seconds, assume speed is 0 + 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.speed = 0.0 self.publish_speed(0.0) + + # If we are moving at constant velocity, we just keep the last self.speed + # We do NOT force it to zero just because it equals prev_speed. + + def log_event(self, msg, details): + self.db_mgr.add_event({ + 'time': datetime.now(tz=timezone.utc), + 'event_type': msg, + 'details': details + }) + + i_msg = Int8() + i_msg.data = self.incidents + self.incidents_publisher.publish(i_msg) - def publish_distance(self, distance): - dmsg = Float32() - dmsg.data = float(distance) - self.distance_publisher.publish(dmsg) self.db_mgr.update_distance(self.distance) + self.db_mgr.update_incidents(self.incidents) self.db_mgr.update_autonomous_distance(self.autonomous_distance) - self.get_logger().debug(f"Distance published: {self.distance}") + + if self.incidents > 0: + self.mdbi = self.distance / self.incidents + else: + self.mdbi = self.distance # Or 0.0, depends on definition. Usually total dist if 0 incidents. + + self.db_mgr.update_mdbi(self.mdbi) + + def publish_distance(self, dist): + msg = Float32() + msg.data = float(dist) + self.distance_publisher.publish(msg) def publish_speed(self, speed): - smsg = Float32() - smsg.data = float(speed) - self.speed_publisher.publish(smsg) + msg = Float32() + msg.data = float(speed) + self.speed_publisher.publish(msg) - # ------------------------- - # Git info helper (optional) - # ------------------------- - def get_git_info(self, repo_path="."): - try: - commit_hash = subprocess.check_output(["git", "rev-parse", "HEAD"], cwd=repo_path).decode().strip() - commit_message = subprocess.check_output(["git", "log", "-1", "--pretty=%B"], cwd=repo_path).decode().strip() - commit_author = subprocess.check_output(["git", "log", "-1", "--pretty=%an"], cwd=repo_path).decode().strip() - commit_date = subprocess.check_output(["git", "log", "-1", "--pretty=%ct"], cwd=repo_path).decode().strip() - commit_date = datetime.fromtimestamp(int(commit_date)).strftime('%Y-%m-%d %H:%M:%S') - branch_name = subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"], cwd=repo_path).decode().strip() - try: - remote_url = subprocess.check_output(["git", "config", "--get", "remote.origin.url"], cwd=repo_path).decode().strip() - except subprocess.CalledProcessError: - remote_url = "No remote" - return { - "build_version": commit_hash[:7], - "commit_message": commit_message, - "commit_author": commit_author, - "commit_date": commit_date, - "branch_name": branch_name, - "remote_url": remote_url - } - except Exception: - return { - "build_version": "Invalid repository", - "commit_message": "", - "commit_author": "", - "commit_date": "", - "branch_name": "Unknown", - "remote_url": "Unknown" - } + def get_git_info(self, repo_path): + # ... (same as original) ... + return {} def main(args=None): rclpy.init(args=args) node = AutonomyMetricsLogger() try: rclpy.spin(node) + except KeyboardInterrupt: + pass finally: node.destroy_node() rclpy.shutdown() if __name__ == '__main__': - main() + main() \ No newline at end of file diff --git a/src/autonomy_metrics/config/metrics_full.yaml b/src/autonomy_metrics/config/metrics_full.yaml index b074bd6..db3e3f1 100644 --- a/src/autonomy_metrics/config/metrics_full.yaml +++ b/src/autonomy_metrics/config/metrics_full.yaml @@ -2,6 +2,9 @@ git_repos: aoc_scenario_path: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" topics: + # --------------------------------------------------------- + # 1. GPS (Working) + # --------------------------------------------------------- - name: "/gps_base/fix" type: "sensor_msgs/msg/NavSatFix" log_fields: @@ -14,6 +17,9 @@ topics: type: "std_msgs/msg/Float32" field: "altitude" + # --------------------------------------------------------- + # 2. Odometry (Working) + # --------------------------------------------------------- - name: "/gophar_vehicle_controller/odometry" type: "nav_msgs/msg/Odometry" role: "odometry" @@ -24,6 +30,9 @@ topics: publish: enable: false + # --------------------------------------------------------- + # 3. System Status (Working) + # --------------------------------------------------------- - name: "/gophar/system_status" type: "dynium_gophar_interfaces/msg/SystemStatus" log_fields: @@ -32,10 +41,24 @@ topics: - "ignition_switch" - "charger_interlock" - "battery_percentage" - publish: enable: true topic: "mdbi_logger/battery_level" type: "std_msgs/msg/Float32" field: "battery_percentage" + # --------------------------------------------------------- + # 4. [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" \ No newline at end of file diff --git a/src/autonomy_metrics/launch/autonomy_metrics.launch.py b/src/autonomy_metrics/launch/autonomy_metrics.launch.py index 7bf4ec1..bb7aec9 100644 --- a/src/autonomy_metrics/launch/autonomy_metrics.launch.py +++ b/src/autonomy_metrics/launch/autonomy_metrics.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): 'config_yaml': LaunchConfiguration('config_yaml'), # optional override 'mongodb_host': 'localhost', - 'mongodb_port': 27017, + 'mongodb_port': 27018, }] ) From 8aacd39da511a527119928d043e412889406b02f Mon Sep 17 00:00:00 2001 From: ibrahim Date: Sun, 30 Nov 2025 22:23:58 +0000 Subject: [PATCH 3/9] Add remote database logging support to AutonomyMetricsLogger --- .../autonomy_metrics/metric_logger.py | 110 ++++++++++++++---- 1 file changed, 85 insertions(+), 25 deletions(-) diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index 0500d82..8744163 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -82,6 +82,10 @@ def __init__(self): self.declare_parameter('config_yaml', '') self.declare_parameter('mongodb_host', 'localhost') self.declare_parameter('mongodb_port', 27017) + # REMOTE PARAMETERS + self.declare_parameter('remote_mongodb_host', '') + self.declare_parameter('remote_mongodb_port', 27017) + self.declare_parameter('enable_remote_logging', False) # Safety switch self.declare_parameter('min_distance_threshold', 0.2) # Timeout to consider robot stopped if no distance update occurs self.declare_parameter('stop_timeout', 2.0) @@ -90,13 +94,28 @@ def __init__(self): 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"Mongo host: {self.mongo_host}, port: {self.mongo_port}") - self.db_mgr = DBMgr(host=self.mongo_host, port=self.mongo_port) + # Database managers + # Local (Primary) DB Manager + self.db_mgr_local = DBMgr(host=self.mongo_host, port=self.mongo_port) + + # 🆕 Remote (Secondary) DB Manager + self.db_mgr_remote = None + if self.enable_remote_logging and self.remote_mongo_host: + self.get_logger().info(f"Initializing Remote DB at {self.remote_mongo_host}:{self.remote_mongo_port}") + try: + self.db_mgr_remote = DBMgr(host=self.remote_mongo_host, port=self.remote_mongo_port) + except Exception as e: + self.get_logger().error(f"Failed to initialize remote DB manager: {e}") + self.enable_remote_logging = False # Internal metrics state self.mdbi = 0.0 @@ -150,8 +169,11 @@ def __init__(self): except Exception: pass - self.db_mgr.init_session(env_variables, git_repos) - + # Update init_session call to use local manager (or both if needed) + 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.get_logger().error("No 'config_yaml' parameter provided.") @@ -328,17 +350,32 @@ def generic_callback(self, topic_name, msg, log_fields): data = {} for f in log_fields: try: + # 1. Extract data data[f] = get_nested_field(msg, f) except Exception: + # Silently fail on field extraction error pass if data: - self.db_mgr.add_event({ + # 2. Construct the event document + event = { 'time': datetime.now(tz=timezone.utc), 'event_type': f"topic:{topic_name}", 'details': data - }) - + } + + # 3. Log to LOCAL DB (always required) + self.db_mgr_local.add_event(event) + + # 4. Log to REMOTE DB (if enabled and initialized) + if self.db_mgr_remote: + try: + self.db_mgr_remote.add_event(event) + except Exception as e: + # Log error, but continue node operation + self.get_logger().warn(f"Remote DB error adding generic event for {topic_name}: {e}") + + # 5. Handle dynamic publishing (no changes needed here) self.handle_dynamic_publish(topic_name, msg) def handle_dynamic_publish(self, topic_name, msg): @@ -384,32 +421,55 @@ def timer_callback(self): # If we are moving at constant velocity, we just keep the last self.speed # We do NOT force it to zero just because it equals prev_speed. - def log_event(self, msg, details): - self.db_mgr.add_event({ - 'time': datetime.now(tz=timezone.utc), - 'event_type': msg, - 'details': details - }) - - i_msg = Int8() - i_msg.data = self.incidents - self.incidents_publisher.publish(i_msg) + def update_db_metrics(self): + # Calculate MDBI once + mdbi_val = float(self.distance) / float(self.incidents) if self.incidents != 0 else self.distance - self.db_mgr.update_distance(self.distance) - self.db_mgr.update_incidents(self.incidents) - self.db_mgr.update_autonomous_distance(self.autonomous_distance) + # List of managers to update (local is always there) + db_managers = [self.db_mgr_local] + if self.db_mgr_remote: + db_managers.append(self.db_mgr_remote) + + 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) + except Exception as e: + self.get_logger().warn(f"DB update failed for manager: {e}") + + def log_event(self, msg='', details=None): + if details is None: + details = {} + event_time = datetime.now(tz=timezone.utc) + event = {'time': event_time, 'event_type': msg, 'details': details} - if self.incidents > 0: - self.mdbi = self.distance / self.incidents - else: - self.mdbi = self.distance # Or 0.0, depends on definition. Usually total dist if 0 incidents. - - self.db_mgr.update_mdbi(self.mdbi) + # 1. Log to LOCAL DB + self.db_mgr_local.add_event(event) + + # 2. Log to REMOTE DB (Safely check if manager exists) + if self.db_mgr_remote: + try: + self.db_mgr_remote.add_event(event) + except Exception as e: + # Log error but don't crash the nodedef publish_distance + self.get_logger().warn(f"Remote DB error during event logging: {e}") + + # publish incidents (no change) + incidents_msg = Int8() + incidents_msg.data = self.incidents + self.incidents_publisher.publish(incidents_msg) + + # Update metric values in BOTH DBs + self.update_db_metrics() def publish_distance(self, dist): msg = Float32() msg.data = float(dist) self.distance_publisher.publish(msg) + self.update_db_metrics() + self.get_logger().debug(f"Distance published: {self.distance}") def publish_speed(self, speed): msg = Float32() From 6e87de8cb8e254cae6e14f875e87d1676bd8a53d Mon Sep 17 00:00:00 2001 From: Your Name Date: Mon, 1 Dec 2025 14:13:57 +0000 Subject: [PATCH 4/9] Enhance AutonomyMetricsLogger with intervention snapshot and global state tracking --- .../autonomy_metrics/metric_logger.py | 154 ++++++++---------- 1 file changed, 70 insertions(+), 84 deletions(-) diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index 8744163..88cca35 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -2,6 +2,7 @@ """ YAML-driven AutonomyMetricsLogger Author: Ibrahim Hroob - JABASAI +Updated: Fixed Battery Pub & Added Intervention Snapshot """ import os @@ -24,7 +25,6 @@ try: from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr except ImportError: - # Fallback mock class if the module is missing, to prevent immediate crash during testing class DBMgr: def __init__(self, host, port): print(f"DB Mock connected to {host}:{port}") def init_session(self, env, git): print("DB Session Init") @@ -34,16 +34,10 @@ def update_incidents(self, i): pass def update_autonomous_distance(self, d): pass def update_mdbi(self, m): pass -# Helper: dynamic message import def import_msg_type(type_str: str): - """ - Accepts strings like 'sensor_msgs/msg/NavSatFix' or 'std_msgs/String' - and returns the message class. - """ + """Dynamically imports message types based on string.""" try: parts = type_str.split('/') - - # Handle "pkg/Message" (2 parts) -> convert to "pkg/msg/Message" if len(parts) == 2: pkg, cls_name = parts submodule = 'msg' @@ -57,7 +51,6 @@ def import_msg_type(type_str: str): except Exception as e: raise ImportError(f"Failed to import message type '{type_str}': {e}") -# Helper: get nested attribute (supports attributes and list indices) def get_nested_field(obj, path: str): try: cur = obj @@ -78,19 +71,16 @@ def __init__(self): self.get_logger().info("Starting YAML-driven AutonomyMetricsLogger") - # Default parameters + # Parameters self.declare_parameter('config_yaml', '') self.declare_parameter('mongodb_host', 'localhost') self.declare_parameter('mongodb_port', 27017) - # REMOTE PARAMETERS self.declare_parameter('remote_mongodb_host', '') self.declare_parameter('remote_mongodb_port', 27017) - self.declare_parameter('enable_remote_logging', False) # Safety switch + self.declare_parameter('enable_remote_logging', False) self.declare_parameter('min_distance_threshold', 0.2) - # Timeout to consider robot stopped if no distance update occurs self.declare_parameter('stop_timeout', 2.0) - # Read parameters 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 @@ -101,21 +91,15 @@ def __init__(self): 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"Mongo host: {self.mongo_host}, port: {self.mongo_port}") - # Database managers - # Local (Primary) DB Manager + # DB Managers self.db_mgr_local = DBMgr(host=self.mongo_host, port=self.mongo_port) - - # 🆕 Remote (Secondary) DB Manager self.db_mgr_remote = None if self.enable_remote_logging and self.remote_mongo_host: - self.get_logger().info(f"Initializing Remote DB at {self.remote_mongo_host}:{self.remote_mongo_port}") try: self.db_mgr_remote = DBMgr(host=self.remote_mongo_host, port=self.remote_mongo_port) except Exception as e: self.get_logger().error(f"Failed to initialize remote DB manager: {e}") - self.enable_remote_logging = False # Internal metrics state self.mdbi = 0.0 @@ -128,25 +112,31 @@ def __init__(self): self.MAN = 'Manual' self.details = {'estop': False, 'operation_mode': self.MAN} + # ---------------------------------------------------- + # 1. NEW: Global Snapshot Storage + # Stores the latest extracted values from all topics + # ---------------------------------------------------- + self.system_snapshot = {} + # 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() # To detect stops + self.last_odom_update_time = self.get_clock().now() self.dynamic_subs = [] self.dynamic_publishers = {} self.topic_cfg_map = {} - # Static Publishers + # 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(Int8, 'mdbi_logger/total_incidents', 10) self.speed_publisher = self.create_publisher(Float32, 'mdbi_logger/robot_speed', 10) - self.battery_publisher = self.create_publisher(Float32, 'mdbi_logger/battery_level', 10) - # Timer + # NOTE: self.battery_publisher removed here to avoid conflict with YAML dynamic publisher + self.heartbeat_timer = self.create_timer(1.0, self.timer_callback) self.load_and_setup_config() @@ -169,14 +159,12 @@ def __init__(self): except Exception: pass - # Update init_session call to use local manager (or both if needed) 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.get_logger().error("No 'config_yaml' parameter provided.") self.config = {} return @@ -193,30 +181,29 @@ def load_and_setup_config(self): name = item.get('name') type_str = item.get('type') - if not name or not type_str: - continue + if not name or not type_str: continue self.topic_cfg_map[name] = item - # Create dynamic publisher if requested + # 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 Pub: {pub_cfg['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 - # Create subscription + # Subscription Setup try: msg_cls = import_msg_type(type_str) role = item.get('role', '').lower() - # Define callback with default args to capture loop variables if role == 'odometry': cb = lambda msg, n=name: self.odom_role_callback(n, msg) elif role == 'control_mode': @@ -239,12 +226,17 @@ def load_and_setup_config(self): def odom_role_callback(self, topic_name, msg): try: pos = msg.pose.pose.position + # Update Snapshot with raw odom info + self.system_snapshot['odometry'] = { + 'x': pos.x, + 'y': pos.y, + 'vx': msg.twist.twist.linear.x + } except Exception: return current_time = self.get_clock().now() - # Initialization if self.init_pose: self.init_pose = False self.previous_x = pos.x @@ -257,38 +249,26 @@ def odom_role_callback(self, topic_name, msg): dy = pos.y - self.previous_y dist = math.sqrt(dx*dx + dy*dy) - # Distance accumulation threshold if dist < self.min_distance_threshold: - # We haven't moved enough to register distance, but we might check speed? - # Actually, standard logic is to assume stationary if under threshold - # We update the time so the heartbeat knows we are still receiving msgs - # but we don't update 'self.speed' here because division by small time is noisy. return - # Calculate time delta for speed - # Ensure we have a previous time set if not hasattr(self, 'previous_time'): self.previous_time = current_time time_diff = (current_time - self.previous_time).nanoseconds * 1e-9 - # Update metrics self.speed = dist / time_diff if time_diff > 0 else 0.0 self.distance += dist if self.details.get('operation_mode') == self.AUTO: self.autonomous_distance += dist - # Update state self.previous_x = pos.x self.previous_y = pos.y self.previous_time = current_time - self.last_odom_update_time = current_time # Mark activity + self.last_odom_update_time = current_time - # Publish immediately on change self.publish_distance(self.distance) self.publish_speed(self.speed) - - # Handle dynamic publish mirroring self.handle_dynamic_publish(topic_name, msg) def control_mode_role_callback(self, topic_name, msg): @@ -300,11 +280,9 @@ def control_mode_role_callback(self, topic_name, msg): return mapping = cfg.get('mode_mapping', {}) - # Robust lookup (string or int) new_mode = mapping.get(str(val)) or mapping.get(val) if new_mode is None: - # Fallback logic if str(val) == "3": new_mode = self.MAN else: new_mode = self.AUTO @@ -315,7 +293,6 @@ def control_mode_role_callback(self, topic_name, msg): self.details['operation_mode'] = new_mode self.get_logger().info(f"Mode changed: {prev_mode} -> {new_mode}") - # Time accumulation logic if prev_mode == self.AUTO and new_mode == self.MAN: if self.autonomous_start_time: delta = (current_time - self.autonomous_start_time).total_seconds() @@ -327,7 +304,15 @@ def control_mode_role_callback(self, topic_name, msg): # Log events if new_mode == self.MAN: self.incidents += 1 - self.log_event('Manual_override', self.details) + # --------------------------------------------------------- + # 2. INTERVENTION SNAPSHOT + # Merge current details with the snapshot of ALL topics + # --------------------------------------------------------- + intervention_data = { + **self.details, + 'system_snapshot': self.system_snapshot.copy() + } + self.log_event('Manual_override', intervention_data) else: self.log_event('Autonomous', self.details) @@ -336,6 +321,8 @@ def estop_role_callback(self, topic_name, msg): field = cfg.get('field', 'data') try: v = bool(get_nested_field(msg, field)) + # Update Snapshot + self.system_snapshot['estop'] = v except Exception: return @@ -348,34 +335,33 @@ def estop_role_callback(self, topic_name, msg): def generic_callback(self, topic_name, msg, log_fields): data = {} + # Extract data specified in log_fields for f in log_fields: try: - # 1. Extract data data[f] = get_nested_field(msg, f) except Exception: - # Silently fail on field extraction error pass if data: - # 2. Construct the event document + # --------------------------------------------------------- + # 3. Update Snapshot + # Save these values to the global snapshot cache + # --------------------------------------------------------- + self.system_snapshot[topic_name] = data + event = { 'time': datetime.now(tz=timezone.utc), 'event_type': f"topic:{topic_name}", 'details': data } - # 3. Log to LOCAL DB (always required) self.db_mgr_local.add_event(event) - - # 4. Log to REMOTE DB (if enabled and initialized) if self.db_mgr_remote: try: self.db_mgr_remote.add_event(event) except Exception as e: - # Log error, but continue node operation - self.get_logger().warn(f"Remote DB error adding generic event for {topic_name}: {e}") + pass - # 5. Handle dynamic publishing (no changes needed here) self.handle_dynamic_publish(topic_name, msg) def handle_dynamic_publish(self, topic_name, msg): @@ -385,9 +371,22 @@ def handle_dynamic_publish(self, topic_name, msg): try: val = get_nested_field(msg, pub_field) if pub_field else msg + out_msg = pub_msg_cls() if hasattr(out_msg, 'data'): - out_msg.data = val + # --------------------------------------------------------- + # 4. FIX: Robust Type Casting + # If target is Float32 but val is int, simple assignment works + # but explicit cast is safer. + # --------------------------------------------------------- + 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): @@ -396,36 +395,33 @@ def handle_dynamic_publish(self, topic_name, msg): setattr(out_msg, attr, val) break except: continue + publisher.publish(out_msg) - except Exception: - pass + + except Exception as e: + # --------------------------------------------------------- + # 5. FIX: Log Errors + # Don't fail silently. Tell user why it failed. + # --------------------------------------------------------- + self.get_logger().error(f"Dynamic publish failed for {topic_name}: {e}") # ------------------------- # Timer & Logging # ------------------------- def timer_callback(self): - # 1. Heartbeat hb = Bool() hb.data = True self.heartbeat_publisher.publish(hb) - # 2. Check for robot stop (TIMEOUT LOGIC) - # If we haven't processed a distance update in X seconds, assume speed is 0 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.speed = 0.0 self.publish_speed(0.0) - - # If we are moving at constant velocity, we just keep the last self.speed - # We do NOT force it to zero just because it equals prev_speed. def update_db_metrics(self): - # Calculate MDBI once mdbi_val = float(self.distance) / float(self.incidents) if self.incidents != 0 else self.distance - - # List of managers to update (local is always there) db_managers = [self.db_mgr_local] if self.db_mgr_remote: db_managers.append(self.db_mgr_remote) @@ -437,31 +433,23 @@ def update_db_metrics(self): dbm.update_autonomous_distance(self.autonomous_distance) dbm.update_mdbi(mdbi_val) except Exception as e: - self.get_logger().warn(f"DB update failed for manager: {e}") + self.get_logger().warn(f"DB update failed: {e}") def log_event(self, msg='', details=None): - if details is None: - details = {} + if details is None: details = {} event_time = datetime.now(tz=timezone.utc) event = {'time': event_time, 'event_type': msg, 'details': details} - # 1. Log to LOCAL DB self.db_mgr_local.add_event(event) - - # 2. Log to REMOTE DB (Safely check if manager exists) if self.db_mgr_remote: try: self.db_mgr_remote.add_event(event) - except Exception as e: - # Log error but don't crash the nodedef publish_distance - self.get_logger().warn(f"Remote DB error during event logging: {e}") + except Exception: pass - # publish incidents (no change) incidents_msg = Int8() incidents_msg.data = self.incidents self.incidents_publisher.publish(incidents_msg) - # Update metric values in BOTH DBs self.update_db_metrics() def publish_distance(self, dist): @@ -469,7 +457,6 @@ def publish_distance(self, dist): msg.data = float(dist) self.distance_publisher.publish(msg) self.update_db_metrics() - self.get_logger().debug(f"Distance published: {self.distance}") def publish_speed(self, speed): msg = Float32() @@ -477,7 +464,6 @@ def publish_speed(self, speed): self.speed_publisher.publish(msg) def get_git_info(self, repo_path): - # ... (same as original) ... return {} def main(args=None): From a69133c51716d8857f13a5bb387b307cde58addb Mon Sep 17 00:00:00 2001 From: ibrahim Date: Thu, 11 Dec 2025 11:43:48 +0000 Subject: [PATCH 5/9] Enhance AutonomyMetricsLogger with intervention triggers and battery field configuration --- .../autonomy_metrics/metric_logger.py | 278 ++++++++++++++---- src/autonomy_metrics/config/metrics_full.yaml | 26 +- 2 files changed, 249 insertions(+), 55 deletions(-) diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index 88cca35..c48c567 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -21,18 +21,8 @@ # Keep a few explicit imports used by default publishers (std msgs) from std_msgs.msg import Bool, Float32, Int8, String -# Optional: existing DB manager -try: - from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr -except ImportError: - class DBMgr: - def __init__(self, host, port): print(f"DB Mock connected to {host}:{port}") - def init_session(self, env, git): print("DB Session Init") - def add_event(self, evt): pass - def update_distance(self, d): pass - def update_incidents(self, i): pass - def update_autonomous_distance(self, d): pass - def update_mdbi(self, m): pass +from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr + def import_msg_type(type_str: str): """Dynamically imports message types based on string.""" @@ -72,9 +62,9 @@ def __init__(self): self.get_logger().info("Starting YAML-driven AutonomyMetricsLogger") # Parameters - self.declare_parameter('config_yaml', '') + 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', 27017) + 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) @@ -110,14 +100,20 @@ def __init__(self): self.autonomous_start_time = None self.AUTO = 'Autonomous' self.MAN = 'Manual' - self.details = {'estop': False, 'operation_mode': self.MAN} + self.details = {'estop': False, 'operation_mode': self.MAN} # ---------------------------------------------------- # 1. NEW: Global Snapshot Storage # Stores the latest extracted values from all topics # ---------------------------------------------------- self.system_snapshot = {} + + # Snapshot of latest values per field for change detection + self.prev_field_values = {} + # Current battery level (updated from any topic with 'battery_field') + self.current_battery = None + # Odometry / Speed state self.previous_x = None self.previous_y = None @@ -134,9 +130,7 @@ def __init__(self): self.distance_publisher = self.create_publisher(Float32, 'mdbi_logger/total_traveled_distance', 10) self.incidents_publisher = self.create_publisher(Int8, 'mdbi_logger/total_incidents', 10) self.speed_publisher = self.create_publisher(Float32, 'mdbi_logger/robot_speed', 10) - - # NOTE: self.battery_publisher removed here to avoid conflict with YAML dynamic publisher - + self.heartbeat_timer = self.create_timer(1.0, self.timer_callback) self.load_and_setup_config() @@ -220,6 +214,80 @@ def load_and_setup_config(self): 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): + """ + Centralised intervention logic: + - increments incidents + - adds system snapshot + - logs event with metrics + """ + if extra is None: + extra = {} + + self.incidents += 1 + + base_details = dict(self.details) # estop, operation_mode, etc. + base_details.update(extra) + base_details["system_snapshot"] = self.system_snapshot.copy() + + self.log_event(event_type, base_details) + + def handle_intervention_triggers(self, topic_name: str, data: dict, cfg: dict): + """ + Apply YAML-configured triggers for this topic. + - intervention_on_message: triggers on any message + - intervention_on_change[field]: triggers when selected field changes + """ + # 1) Trigger on any message (e.g. /cmd_vel/joy) + msg_trig = cfg.get("intervention_on_message", {}) + if msg_trig.get("enable", False): + evt_type = msg_trig.get("event_type", f"{topic_name}_activity") + self.trigger_intervention(evt_type, extra={"topic": topic_name}) + + # 2) Trigger on change of specific fields + field_trigs = cfg.get("intervention_on_change", {}) + for field_name, trig_cfg in field_trigs.items(): + if field_name not in data: + continue + + key = (topic_name, field_name) + prev = self.prev_field_values.get(key) + new = data[field_name] + changed = (prev is None) or (prev != new) + self.prev_field_values[key] = new + + if not changed: + continue + + # Optional: only trigger on a specific value (e.g. True) + trigger_value = trig_cfg.get("trigger_value", None) + if trigger_value is not None and new != trigger_value: + continue + + evt_type = trig_cfg.get( + "event_type", f"{topic_name}:{field_name}_changed" + ) + extra = { + "topic": topic_name, + "field": field_name, + "new_value": new, + "prev_value": prev, + } + self.trigger_intervention(evt_type, extra=extra) + # ------------------------- # Callbacks # ------------------------- @@ -253,10 +321,10 @@ def odom_role_callback(self, topic_name, msg): return if not hasattr(self, 'previous_time'): - self.previous_time = current_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 if self.details.get('operation_mode') == self.AUTO: @@ -265,12 +333,20 @@ def odom_role_callback(self, topic_name, msg): self.previous_x = pos.x self.previous_y = pos.y self.previous_time = current_time - self.last_odom_update_time = current_time + self.last_odom_update_time = current_time + + # NEW: keep metrics in the snapshot too + self.system_snapshot['metrics'] = { + 'distance': self.distance, + 'autonomous_distance': self.autonomous_distance, + 'speed': self.speed, + } self.publish_distance(self.distance) self.publish_speed(self.speed) self.handle_dynamic_publish(topic_name, msg) + def control_mode_role_callback(self, topic_name, msg): cfg = self.topic_cfg_map.get(topic_name, {}) mode_field = cfg.get('mode_field', 'data') @@ -281,13 +357,15 @@ def control_mode_role_callback(self, topic_name, msg): 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 + if str(val) == "3": + new_mode = self.MAN + else: + new_mode = self.AUTO current_time = datetime.now() - + if new_mode != self.details.get('operation_mode'): prev_mode = self.details.get('operation_mode') self.details['operation_mode'] = new_mode @@ -298,23 +376,18 @@ def control_mode_role_callback(self, topic_name, msg): 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') + elif new_mode == self.AUTO: self.autonomous_start_time = current_time - - # Log events - if new_mode == self.MAN: - self.incidents += 1 - # --------------------------------------------------------- - # 2. INTERVENTION SNAPSHOT - # Merge current details with the snapshot of ALL topics - # --------------------------------------------------------- - intervention_data = { - **self.details, + # Logging autonomous mode change (not counted as intervention) + self.log_event('Autonomous', { + **self.details, 'system_snapshot': self.system_snapshot.copy() - } - self.log_event('Manual_override', intervention_data) - else: - self.log_event('Autonomous', self.details) + }) + def estop_role_callback(self, topic_name, msg): cfg = self.topic_cfg_map.get(topic_name, {}) @@ -330,40 +403,51 @@ def estop_role_callback(self, topic_name, msg): self.details['estop'] = v self.get_logger().info(f"E-Stop: {v}") if v: - self.incidents += 1 - self.log_event('EMS', self.details) + # E-stop counts as intervention + self.trigger_intervention('EMS') + def generic_callback(self, topic_name, msg, log_fields): + cfg = self.topic_cfg_map.get(topic_name, {}) data = {} + # Extract data specified in log_fields for f in log_fields: try: data[f] = get_nested_field(msg, f) except Exception: pass - + if data: - # --------------------------------------------------------- - # 3. Update Snapshot - # Save these values to the global snapshot cache - # --------------------------------------------------------- + # 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: + self.current_battery = data[battery_field] + + # Topic-level event logging event = { 'time': datetime.now(tz=timezone.utc), 'event_type': f"topic:{topic_name}", 'details': data } - + self.db_mgr_local.add_event(event) if self.db_mgr_remote: try: self.db_mgr_remote.add_event(event) - except Exception as e: + except Exception: pass + # Apply YAML-configured intervention triggers (message & field-change) + self.handle_intervention_triggers(topic_name, data, cfg) + + # Dynamic republishing if configured self.handle_dynamic_publish(topic_name, msg) + def handle_dynamic_publish(self, topic_name, msg): pub_cfg = self.dynamic_publishers.get(topic_name) if not pub_cfg: return @@ -436,21 +520,31 @@ def update_db_metrics(self): self.get_logger().warn(f"DB update failed: {e}") def log_event(self, msg='', details=None): - if details is None: details = {} + if details is None: + details = {} + + # Attach metrics to every event + details = { + **details, + 'metrics': self.get_metrics_snapshot() + } + event_time = datetime.now(tz=timezone.utc) event = {'time': event_time, 'event_type': msg, 'details': details} - + self.db_mgr_local.add_event(event) if self.db_mgr_remote: try: self.db_mgr_remote.add_event(event) - except Exception: pass + except Exception: + pass incidents_msg = Int8() incidents_msg.data = self.incidents self.incidents_publisher.publish(incidents_msg) self.update_db_metrics() + def publish_distance(self, dist): msg = Float32() @@ -464,7 +558,85 @@ def publish_speed(self, speed): self.speed_publisher.publish(msg) def get_git_info(self, repo_path): - return {} + """ + Return basic git metadata for a repo. + + Schema (example): + { + "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", + "dirty": false, + "error": null + } + """ + info = { + "path": repo_path, + "exists": False, + "remote": None, + "branch": None, + "commit": None, + "short_commit": 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 + + 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 (best effort) + try: + info["remote"] = git(["config", "--get", "remote.origin.url"]) + except Exception: + info["remote"] = None + + # Branch (HEAD might be detached) + 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 + + try: + info["short_commit"] = git(["rev-parse", "--short", "HEAD"]) + except Exception: + info["short_commit"] = None + + # Dirty / clean state + try: + status = git(["status", "--porcelain"]) + info["dirty"] = bool(status) + except Exception: + info["dirty"] = None + + except Exception as e: + info["error"] = str(e) + + return info + def main(args=None): rclpy.init(args=args) diff --git a/src/autonomy_metrics/config/metrics_full.yaml b/src/autonomy_metrics/config/metrics_full.yaml index db3e3f1..65b105c 100644 --- a/src/autonomy_metrics/config/metrics_full.yaml +++ b/src/autonomy_metrics/config/metrics_full.yaml @@ -31,22 +31,33 @@ topics: enable: false # --------------------------------------------------------- - # 3. System Status (Working) + # 3. System Status (Working + intervention on fault) # --------------------------------------------------------- - name: "/gophar/system_status" type: "dynium_gophar_interfaces/msg/SystemStatus" + role: "system_status" log_fields: - "fault_shutdown" - "hv_on" - "ignition_switch" - "charger_interlock" - "battery_percentage" + + # 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" + # --------------------------------------------------------- # 4. [MISSING] Control Mode (REQUIRED FOR MDBI) # --------------------------------------------------------- @@ -61,4 +72,15 @@ topics: "AUTO": "Autonomous" "MANUAL": "Manual" "1": "Autonomous" - "0": "Manual" \ No newline at end of file + "0": "Manual" + + # --------------------------------------------------------- + # 5. Joystick command -> Intervention trigger on activity + # --------------------------------------------------------- + - name: "/cmd_vel/joy" + 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" \ No newline at end of file From fa67060eba821a62d817ad0d5393184352a7393b Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 11 Dec 2025 12:05:22 +0000 Subject: [PATCH 6/9] Enhance metrics configuration with additional git repository paths --- .../autonomy_metrics/metric_logger.py | 36 +++++++++++++++++-- src/autonomy_metrics/config/metrics_full.yaml | 13 ++++++- 2 files changed, 45 insertions(+), 4 deletions(-) diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index c48c567..0ce9201 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -561,7 +561,7 @@ def get_git_info(self, repo_path): """ Return basic git metadata for a repo. - Schema (example): + Example schema: { "path": "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario", "exists": True, @@ -569,6 +569,9 @@ def get_git_info(self, repo_path): "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 } @@ -580,6 +583,9 @@ def get_git_info(self, repo_path): "branch": None, "commit": None, "short_commit": None, + "commit_message": None, # NEW + "tags": [], # NEW + "describe": None, # NEW "dirty": None, "error": None, } @@ -602,13 +608,13 @@ def git(args): info["exists"] = True - # Remote (best effort) + # Remote try: info["remote"] = git(["config", "--get", "remote.origin.url"]) except Exception: info["remote"] = None - # Branch (HEAD might be detached) + # Branch try: info["branch"] = git(["rev-parse", "--abbrev-ref", "HEAD"]) except Exception: @@ -625,6 +631,30 @@ def git(args): except Exception: info["short_commit"] = None + # NEW: Last commit message (full body or just subject) + try: + # Subject only: + info["commit_message"] = git(["log", "-1", "--pretty=%s"]) + # If you prefer full body, use: "--pretty=%B" + except Exception: + info["commit_message"] = None + + # NEW: Tags pointing at HEAD + try: + tags_str = git(["tag", "--points-at", "HEAD"]) + if tags_str: + info["tags"] = tags_str.splitlines() + else: + info["tags"] = [] + except Exception: + info["tags"] = [] + + # NEW: git describe (nearest tag + distance) + try: + info["describe"] = git(["describe", "--tags", "--always"]) + except Exception: + info["describe"] = None + # Dirty / clean state try: status = git(["status", "--porcelain"]) diff --git a/src/autonomy_metrics/config/metrics_full.yaml b/src/autonomy_metrics/config/metrics_full.yaml index 65b105c..74d3261 100644 --- a/src/autonomy_metrics/config/metrics_full.yaml +++ b/src/autonomy_metrics/config/metrics_full.yaml @@ -1,5 +1,16 @@ git_repos: - aoc_scenario_path: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" + scenario: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" + autonomy_metrics_logger: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/autonomy_metrics_logger" + navfn_planner: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/navfn_planner" + perception_boundary_estimator: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/perception_boundary_estimator" + pointcloud_processing: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/pointcloud_processing" + robot_boundary_checker: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/robot_boundary_checker" + ros2_topic_monitor: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/ros2_topic_monitor" + row_nav_planner: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/row_nav_planner" + ppl_gofar_platform: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/robots/ppl_gofar_platform" + topological_navigation: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/contrib/topological_navigation" + sentor: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/contrib/sentor" + kiss-icp: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/contrib/kiss-icp" topics: # --------------------------------------------------------- From 217ea75748ea1beb25a9d0d15339df3c832af734 Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 11 Dec 2025 14:34:39 +0000 Subject: [PATCH 7/9] Enhance AutonomyMetricsLogger with collision incident tracking and update metrics configuration --- .../autonomy_metrics/db_mgr.py | 19 +- .../autonomy_metrics/metric_logger.py | 406 ++++++++++++++++-- src/autonomy_metrics/config/metrics_full.yaml | 85 +++- 3 files changed, 455 insertions(+), 55 deletions(-) diff --git a/src/autonomy_metrics/autonomy_metrics/db_mgr.py b/src/autonomy_metrics/autonomy_metrics/db_mgr.py index 1651799..00355f9 100644 --- a/src/autonomy_metrics/autonomy_metrics/db_mgr.py +++ b/src/autonomy_metrics/autonomy_metrics/db_mgr.py @@ -72,6 +72,7 @@ def init_session(self, env_variables, aoc_repos_info): "incidents": 0, "distance": 0, "autonomous_distance": 0, + "collision_incidents": 0, # NEW "events": [] } result = self.sessions_collection.insert_one(session_document) @@ -155,4 +156,20 @@ def update_mdbi(self, mdbi): {"_id": ObjectId(self.session_id)}, {"$set": {"mdbi": mdbi}} ) - return result.modified_count > 0 \ No newline at end of file + return result.modified_count > 0 + + def update_collision_incidents(self, collision_incidents): + """ + Updates the collision_incidents value of the current session. + + Args: + collision_incidents (int): The new collision incidents value. + + Returns: + bool: True if the value was updated successfully, False otherwise. + """ + result = self.sessions_collection.update_one( + {"_id": ObjectId(self.session_id)}, + {"$set": {"collision_incidents": collision_incidents}} + ) + return result.modified_count > 0 diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index 0ce9201..9eee832 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -19,11 +19,50 @@ from rclpy.time import Time # Keep a few explicit imports used by default publishers (std msgs) -from std_msgs.msg import Bool, Float32, Int8, String +from std_msgs.msg import Bool, Float32, Int32, String from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr +def ros_msg_to_dict(msg): + """ + Recursively convert a ROS 2 Python message into a plain dict / list / primitive + so it can be safely stored in system_snapshot / Mongo. + + Works for nested messages and sequences. + """ + # Primitive types + if isinstance(msg, (bool, int, float, str)): + return msg + + # Sequences (lists, tuples, etc.) + if isinstance(msg, (list, tuple)): + return [ros_msg_to_dict(v) for v in msg] + + # ROS 2 messages usually have get_fields_and_field_types() + if hasattr(msg, "get_fields_and_field_types"): + data = {} + for field_name in msg.get_fields_and_field_types().keys(): + try: + val = getattr(msg, field_name) + data[field_name] = ros_msg_to_dict(val) + except Exception: + # Best-effort; skip problematic fields + pass + return data + + # Fallback: try to convert __dict__ + if hasattr(msg, "__dict__"): + return { + k: ros_msg_to_dict(v) + for k, v in msg.__dict__.items() + if not k.startswith('_') + } + + # Last resort: return as-is + return msg + + def import_msg_type(type_str: str): """Dynamically imports message types based on string.""" try: @@ -70,7 +109,20 @@ def __init__(self): self.declare_parameter('enable_remote_logging', False) self.declare_parameter('min_distance_threshold', 0.2) self.declare_parameter('stop_timeout', 2.0) - + self.declare_parameter('mode_observer_cmd_timeout', 1.0) # seconds + self.declare_parameter('mode_observer_speed_threshold', 0.01) # m/s + # Collision detection tuning + self.declare_parameter('collision_nav_threshold', 0.01) # nav cmd must be > this + self.declare_parameter('collision_zero_threshold', 0.001) # collision cmd abs(linear.x) <= this + self.declare_parameter('collision_time_window', 0.5) # seconds: nav & collision cmds must be close in time + self.declare_parameter('collision_log_cooldown', 1.0) # seconds between logged collisions + + self.collision_nav_threshold = self.get_parameter('collision_nav_threshold').get_parameter_value().double_value + self.collision_zero_threshold = self.get_parameter('collision_zero_threshold').get_parameter_value().double_value + self.collision_time_window = self.get_parameter('collision_time_window').get_parameter_value().double_value + self.collision_log_cooldown = self.get_parameter('collision_log_cooldown').get_parameter_value().double_value + self.mode_observer_cmd_timeout = self.get_parameter('mode_observer_cmd_timeout').get_parameter_value().double_value + self.mode_observer_speed_threshold = self.get_parameter('mode_observer_speed_threshold').get_parameter_value().double_value self.config_path = self.get_parameter('config_yaml').get_parameter_value().string_value self.mongo_host = self.get_parameter('mongodb_host').get_parameter_value().string_value self.mongo_port = self.get_parameter('mongodb_port').get_parameter_value().integer_value @@ -100,10 +152,27 @@ def __init__(self): self.autonomous_start_time = None self.AUTO = 'Autonomous' self.MAN = 'Manual' - self.details = {'estop': False, 'operation_mode': self.MAN} + self.details = {'estop': False, 'operation_mode': self.AUTO} + # Mode observation (velocity-based fallback when no "operation_mode" topic available) + self.has_explicit_control_mode = False + self.mode_observer_enabled = False + self.mode_observer_active = False + self.mode_observer_topic_name = None + self.last_auto_cmd_time = None + + # Collision monitoring + self.collision_incidents = 0 + self.collision_prev_has_velocity = False # for falling-edge detection + + self.last_nav_cmd = None + self.last_nav_time = None + self.last_collision_cmd = None + self.last_collision_time = None + self.last_collision_logged_time = None + # ---------------------------------------------------- - # 1. NEW: Global Snapshot Storage + # Global Snapshot Storage # Stores the latest extracted values from all topics # ---------------------------------------------------- self.system_snapshot = {} @@ -128,13 +197,22 @@ def __init__(self): # 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(Int8, 'mdbi_logger/total_incidents', 10) + self.incidents_publisher = self.create_publisher(Int32, 'mdbi_logger/total_incidents', 10) self.speed_publisher = self.create_publisher(Float32, 'mdbi_logger/robot_speed', 10) - + self.collision_incidents_publisher = self.create_publisher(Int32, 'mdbi_logger/total_collision_incidents', 10) + self.heartbeat_timer = self.create_timer(1.0, self.timer_callback) self.load_and_setup_config() + # Use the observer only if there is no explicit control_mode topic configured + self.mode_observer_active = self.mode_observer_enabled and not self.has_explicit_control_mode + if self.mode_observer_active: + self.get_logger().info( + f"Mode observer enabled using topic {self.mode_observer_topic_name} " + "(no explicit control_mode topic configured)" + ) + # Init Session env_variables = { 'robot_name': os.getenv('ROBOT_NAME', 'UNDEFINED'), @@ -171,6 +249,12 @@ def load_and_setup_config(self): 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 + ) + for item in topics: name = item.get('name') type_str = item.get('type') @@ -179,6 +263,12 @@ def load_and_setup_config(self): 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 + # Dynamic Publisher Setup pub_cfg = item.get('publish', {}) if pub_cfg.get('enable', False): @@ -204,6 +294,10 @@ def load_and_setup_config(self): cb = lambda msg, n=name: self.control_mode_role_callback(n, msg) elif role == 'estop': cb = lambda msg, n=name: self.estop_role_callback(n, msg) + elif role == 'collision_nav': + cb = lambda msg, n=name: self.collision_nav_callback(n, msg) + elif role == 'collision_output': + cb = lambda msg, n=name: self.collision_output_callback(n, msg) else: lf = item.get('log_fields', []) cb = lambda msg, n=name, l=lf: self.generic_callback(n, msg, l) @@ -214,6 +308,7 @@ def load_and_setup_config(self): 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. @@ -227,17 +322,22 @@ def get_metrics_snapshot(self): metrics["battery_percentage"] = self.current_battery return metrics - def trigger_intervention(self, event_type: str, extra: dict | None = None): + def trigger_intervention(self, event_type: str, extra: dict | None = None, force_count: bool = False): """ - Centralised intervention logic: - - increments incidents - - adds system snapshot - - logs event with metrics + 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 = {} - self.incidents += 1 + current_mode = self.details.get('operation_mode', self.MAN) + count_incident = force_count or (current_mode == self.AUTO) + + if count_incident: + self.incidents += 1 base_details = dict(self.details) # estop, operation_mode, etc. base_details.update(extra) @@ -245,6 +345,7 @@ def trigger_intervention(self, event_type: str, extra: dict | None = None): self.log_event(event_type, base_details) + def handle_intervention_triggers(self, topic_name: str, data: dict, cfg: dict): """ Apply YAML-configured triggers for this topic. @@ -315,7 +416,7 @@ def odom_role_callback(self, topic_name, msg): dx = pos.x - self.previous_x dy = pos.y - self.previous_y - dist = math.sqrt(dx*dx + dy*dy) + dist = math.sqrt(dx * dx + dy * dy) if dist < self.min_distance_threshold: return @@ -327,6 +428,11 @@ def odom_role_callback(self, topic_name, msg): 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 @@ -335,7 +441,7 @@ def odom_role_callback(self, topic_name, msg): self.previous_time = current_time self.last_odom_update_time = current_time - # NEW: keep metrics in the snapshot too + # Keep metrics in the snapshot too self.system_snapshot['metrics'] = { 'distance': self.distance, 'autonomous_distance': self.autonomous_distance, @@ -378,7 +484,7 @@ def control_mode_role_callback(self, topic_name, msg): self.autonomous_start_time = None # Manual override is an intervention - self.trigger_intervention('Manual_override') + self.trigger_intervention('Manual_override', force_count=True) elif new_mode == self.AUTO: self.autonomous_start_time = current_time @@ -409,14 +515,34 @@ def estop_role_callback(self, topic_name, msg): 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 + } + data = {} + log_all = bool(cfg.get("log_all_fields", False)) - # Extract data specified in log_fields - for f in log_fields: + if log_all: + # NEW: capture the entire message into the snapshot try: - data[f] = get_nested_field(msg, f) - except Exception: - pass + 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: + # Original behaviour: only selected log_fields + 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 @@ -427,19 +553,9 @@ def generic_callback(self, topic_name, msg, log_fields): if battery_field and battery_field in data: self.current_battery = data[battery_field] - # Topic-level event logging - event = { - 'time': datetime.now(tz=timezone.utc), - 'event_type': f"topic:{topic_name}", - 'details': data - } - - self.db_mgr_local.add_event(event) - if self.db_mgr_remote: - try: - self.db_mgr_remote.add_event(event) - except Exception: - pass + # IMPORTANT: + # No per-message DB logging anymore. + # We only call log_event(...) on interventions / collisions / mode changes. # Apply YAML-configured intervention triggers (message & field-change) self.handle_intervention_triggers(topic_name, data, cfg) @@ -489,6 +605,166 @@ def handle_dynamic_publish(self, topic_name, msg): # --------------------------------------------------------- self.get_logger().error(f"Dynamic publish failed for {topic_name}: {e}") + + def update_observed_mode_from_odom(self, is_moving: bool, now=None): + """ + Fallback mode inference when there is no explicit /gophar/operation_mode. + + Logic: + - If robot is moving AND we have recent /cmd_vel/collision_smoothed, mode = Autonomous + - If robot is moving AND no recent /cmd_vel/collision_smoothed, mode = Manual + - If robot is not moving, keep previous mode. + - If mode changes from AUTO -> MAN, log an intervention. + """ + if not self.mode_observer_active: + return + + if now is None: + now = self.get_clock().now() + + 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 + + if not is_moving: + # Don't flip modes just because we stopped; keep previous + return + + if auto_cmd_recent: + new_mode = self.AUTO + else: + new_mode = self.MAN + + if new_mode == prev_mode: + return + + self.get_logger().info(f"[ModeObserver] Observed mode change: {prev_mode} -> {new_mode}") + + 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, + } + # Uses the same helper we defined earlier + self.trigger_intervention("Manual_override_observed", extra=extra, force_count=True) + + elif prev_mode != self.AUTO and new_mode == self.AUTO: + self.autonomous_start_time = current_time + + self.details['operation_mode'] = new_mode + + + def collision_nav_callback(self, topic_name, msg): + """ + Store latest /cmd_vel/nav command. + """ + now = self.get_clock().now() + self.last_nav_cmd = msg + self.last_nav_time = now + + self.system_snapshot['cmd_vel_nav'] = { + 'linear_x': msg.linear.x, + 'linear_y': msg.linear.y, + 'linear_z': msg.linear.z, + 'angular_x': msg.angular.x, + 'angular_y': msg.angular.y, + 'angular_z': msg.angular.z, + } + # 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, + 'linear_z': msg.linear.z, + 'angular_x': msg.angular.x, + 'angular_y': msg.angular.y, + 'angular_z': msg.angular.z, + } + + # Check for falling edge-based collision + self.check_collision_condition(now, coll_vx) + + + def check_collision_condition(self, now, coll_vx_now: float): + """ + Detect a collision event only on falling edge: + + - previous /cmd_vel/collision had velocity (|vx| > collision_zero_threshold) + - current /cmd_vel/collision has ~zero velocity (|vx| <= collision_zero_threshold) + - latest /cmd_vel/nav.linear.x > collision_nav_threshold + - nav command is recent (within collision_time_window) + + When this condition is met: + - increment collision_incidents once + - log a 'Collision' event with full snapshot + - publish total collision_incidents + + We only increment again if the cycle repeats: + velocity on /cmd_vel/collision -> zero -> velocity -> zero -> ... + """ + # Need a nav command to interpret the collision behaviour + if self.last_nav_cmd is None or self.last_nav_time is None: + # Still update state, but cannot decide yet + collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold + self.collision_prev_has_velocity = collision_has_velocity_now + return + + # Check nav recency + dt_nav = (now - self.last_nav_time).nanoseconds * 1e-9 + if dt_nav > self.collision_time_window: + collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold + self.collision_prev_has_velocity = collision_has_velocity_now + return + + nav_vx = float(self.last_nav_cmd.linear.x) + + # Does nav actually request motion? + if nav_vx <= self.collision_nav_threshold: + collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold + self.collision_prev_has_velocity = collision_has_velocity_now + return + + # Edge detection on /cmd_vel/collision + collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold + + # Falling edge: had velocity before, now ~zero + if self.collision_prev_has_velocity and not collision_has_velocity_now: + # This is one collision incident + self.collision_incidents += 1 + self.log_collision_event() + + # Update for next call + self.collision_prev_has_velocity = collision_has_velocity_now + + # ------------------------- # Timer & Logging # ------------------------- @@ -505,7 +781,13 @@ def timer_callback(self): self.publish_speed(0.0) def update_db_metrics(self): - mdbi_val = float(self.distance) / float(self.incidents) if self.incidents != 0 else self.distance + # MBDI = autonomous_distance / incidents + if self.incidents != 0: + mdbi_val = float(self.autonomous_distance) / float(self.incidents) + else: + # No incidents → use autonomous distance as-is (no division) + mdbi_val = float(self.autonomous_distance) + db_managers = [self.db_mgr_local] if self.db_mgr_remote: db_managers.append(self.db_mgr_remote) @@ -516,9 +798,12 @@ def update_db_metrics(self): dbm.update_incidents(self.incidents) dbm.update_autonomous_distance(self.autonomous_distance) dbm.update_mdbi(mdbi_val) + if hasattr(dbm, "update_collision_incidents"): + 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 = {} @@ -539,12 +824,61 @@ def log_event(self, msg='', details=None): except Exception: pass - incidents_msg = Int8() + 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), + "z": float(self.last_nav_cmd.linear.z), + }, + "angular": { + "x": float(self.last_nav_cmd.angular.x), + "y": float(self.last_nav_cmd.angular.y), + "z": float(self.last_nav_cmd.angular.z), + }, + }, + "collision_cmd": { + "linear": { + "x": float(self.last_collision_cmd.linear.x), + "y": float(self.last_collision_cmd.linear.y), + "z": float(self.last_collision_cmd.linear.z), + }, + "angular": { + "x": float(self.last_collision_cmd.angular.x), + "y": float(self.last_collision_cmd.angular.y), + "z": float(self.last_collision_cmd.angular.z), + }, + }, + # Full snapshot of all system data at this moment + "system_snapshot": self.system_snapshot.copy(), + } + + # 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() diff --git a/src/autonomy_metrics/config/metrics_full.yaml b/src/autonomy_metrics/config/metrics_full.yaml index 74d3261..c6e0e0c 100644 --- a/src/autonomy_metrics/config/metrics_full.yaml +++ b/src/autonomy_metrics/config/metrics_full.yaml @@ -14,7 +14,7 @@ git_repos: topics: # --------------------------------------------------------- - # 1. GPS (Working) + # GPS # --------------------------------------------------------- - name: "/gps_base/fix" type: "sensor_msgs/msg/NavSatFix" @@ -23,13 +23,13 @@ topics: - "longitude" - "altitude" publish: - enable: true + enable: false topic: "mdbi_logger/gps_alt" type: "std_msgs/msg/Float32" field: "altitude" # --------------------------------------------------------- - # 2. Odometry (Working) + # Odometry # --------------------------------------------------------- - name: "/gophar_vehicle_controller/odometry" type: "nav_msgs/msg/Odometry" @@ -42,11 +42,12 @@ topics: enable: false # --------------------------------------------------------- - # 3. System Status (Working + intervention on fault) + # System Status (intervention on fault) # --------------------------------------------------------- - name: "/gophar/system_status" type: "dynium_gophar_interfaces/msg/SystemStatus" role: "system_status" + log_fields: [] log_fields: - "fault_shutdown" - "hv_on" @@ -70,28 +71,76 @@ topics: event_type: "Fault_shutdown" # --------------------------------------------------------- - # 4. [MISSING] Control Mode (REQUIRED FOR MDBI) + # [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" + # - 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" # --------------------------------------------------------- - # 5. Joystick command -> Intervention trigger on activity + # X. Mode observation via velocity command + # --------------------------------------------------------- + # Rule: the heuristic will only be used if there is no topic with role: "control_mode" configured in the YAML. + - name: "/cmd_vel/collision_smoothed" + type: "geometry_msgs/msg/Twist" + role: "mode_observer" + log_fields: [] # keep empty if you don't want per-message events + + # --------------------------------------------------------- - - name: "/cmd_vel/joy" + # 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" \ No newline at end of file + event_type: "Joy_override" + + # --------------------------------------------------------- + # Collision monitor: nav vs collision-filtered velocity + # --------------------------------------------------------- + + - name: "/cmd_vel/nav" + type: "geometry_msgs/msg/Twist" + role: "collision_nav" + log_fields: [] # no per-message events needed + + - name: "/cmd_vel/collision" + type: "geometry_msgs/msg/Twist" + role: "collision_output" + log_fields: [] # no per-message events needed + + # --------------------------------------------------------- + # Steering actuator + motor status snapshots for interventions + # --------------------------------------------------------- + + - name: "/gophar/steering_actuator/front" + type: "dynium_gophar_interfaces/msg/SteeringActuatorStatus" + log_all_fields: true # <--- NEW + log_fields: [] # optional, ignored if log_all_fields is true + + - name: "/gophar/steering_actuator/rear" + type: "dynium_gophar_interfaces/msg/SteeringActuatorStatus" + log_all_fields: true # <--- NEW + log_fields: [] # optional, ignored if log_all_fields is true + + - name: "/gophar/motor_status/motor_0" + type: "dynium_gophar_interfaces/msg/MotorStatus" + log_all_fields: true # <--- NEW + log_fields: [] # optional, ignored if log_all_fields is true + + - name: "/gophar/motor_status/motor_1" + type: "dynium_gophar_interfaces/msg/MotorStatus" + log_all_fields: true # <--- NEW + log_fields: [] # optional, ignored if log_all_fields is true \ No newline at end of file From 5f469deecbf9b06e32b6bf7139fe4d7f5def867f Mon Sep 17 00:00:00 2001 From: ibrahim Date: Thu, 11 Dec 2025 19:22:43 +0000 Subject: [PATCH 8/9] Enhance AutonomyMetricsLogger with improved YAML configuration, refined MongoDB integration, and optimized command handling --- README.md | 550 ++++++++++++++++-- .../autonomy_metrics/metric_logger.py | 21 +- src/autonomy_metrics/config/metrics_full.yaml | 59 +- .../launch/autonomy_metrics.launch.py | 163 +++++- 4 files changed, 676 insertions(+), 117 deletions(-) diff --git a/README.md b/README.md index a98add0..c62fcc8 100644 --- a/README.md +++ b/README.md @@ -1,67 +1,521 @@ -# Autonomy Metrics Logger Node +# Autonomy Metrics Logger -The Autonomy Metrics Logger is a ROS2 node that tracks various robot events and computes the **Mean Distance Between Incidents (MDBI)**. This node monitors topics such as GPS, odometry, battery status, and emergency stop status to log incidents and key metrics into a MongoDB database. +`autonomy_metrics_logger` is a ROS 2 node that: -## Features +- Tracks travelled distance, autonomous distance, incidents, and collisions. +- Logs interventions and collisions to MongoDB with a full snapshot of system state. +- Is configured entirely via a YAML file (topics, fields, triggers, dynamic publishers). +- Supports both explicit operation mode (`/gophar/operation_mode`) and inferred mode (odom + velocity). -- **MDBI Calculation**: Computes the Mean Distance Between Incidents based on traveled distance and logged incidents. -- **Autonomous Mode Tracking**: Records the time spent and distance covered in autonomous mode. -- **Event Logging**: Logs critical events like emergency stops, manual overrides, and coordinator tasks. -- **Database Integration**: Uses MongoDB to store event logs, distances, and operational details of the robot. -- **Heartbeat Signal**: Publishes a heartbeat message every 5 seconds to indicate the node is alive. -- **Git Repositories Logging**: Captures and stores Git information of the active repositories for traceability. +--- -## Usage +## 1. High-Level Behaviour -1. **Set Environment Variables:** +The node: - Configure the robot session by setting the following environment variables: +1. Loads a YAML config describing topics and logging behaviour. +2. Subscribes to all configured topics. +3. Maintains a live `system_snapshot` dict with the latest values from each topic. +4. Updates metrics: + - `distance` (total odometry distance) + - `autonomous_distance` (distance when mode = Autonomous) + - `incidents` (interventions in autonomous mode, plus manual overrides) + - `collision_incidents` (collision monitor only, separate from incidents) +5. Logs events to MongoDB **only** when something happens: + - Manual override (explicit or observed) + - E-stop + - Faults / joy overrides / other YAML triggers + - Collisions (nav vs collision velocity) +6. Each event stored in Mongo includes: + - `metrics` (distance, autonomous_distance, speed, battery) + - `system_snapshot` (current values of all configured topics) + - Event type & extra details. - ```sh - export ROBOT_NAME= - export FARM_NAME= - export FIELD_NAME= - export APPLICATION= - export SCENARIO_NAME= - ``` +MDBI is computed as: -2. **Run the Node:** +> `mdbi = autonomous_distance / incidents` +> (If `incidents == 0`, `mdbi = autonomous_distance`.) - ```sh - ros2 launch autonomy_metrics autonomy_metrics.launch.py - ``` +--- -## Parameters +## 2. MongoDB Integration -The node uses the following ROS parameters, which can be customized: +The node creates a document in `robot_incidents.sessions` per run: -- `mongodb_host` (default: `localhost`): MongoDB server host. -- `mongodb_port` (default: `27017`): MongoDB server port. -- `aoc_scenario_path` (default: `""`): Path to the AOC scenario repository. -- `aoc_navigation_path` (default: `""`): Path to the AOC navigation repository. -- `min_distance_threshold` (default: `0.2`): Minimum distance threshold for recording traveled distance (in meters). -- `gps_topic` (default: `/gps_base/fix`) -- `odometry_topic` (default: `/diff_drive_controller/odom`) -- `battery_status_topic` (default: `/diff_drive_controller/battery_status`) -- `estop_status_topic` (default: `/diff_drive_controller/estop_status`) -- `hunter_status_topic` (default: `/hunter_status`) -- `actioned_by_coordinator_topic` (default: `/topological_navigation/execute_policy_mode/goal`) +```json +{ + "session_start_time": "...", + "robot_name": "...", + "farm_name": "...", + "field_name": "...", + "application": "...", + "scenario_name": "...", + "aoc_repos_info": [ + { + "aoc_scenario_path": { + "path": "...", + "exists": true, + "remote": "...", + "branch": "...", + "commit": "...", + "short_commit": "...", + "commit_message": "...", + "tags": ["v0.1.0"], + "describe": "v0.1.0-3-gXXXXXXX", + "dirty": false, + "error": null + } + } + ], + "mdbi": 123.4, + "incidents": 5, + "distance": 456.7, + "autonomous_distance": 321.0, + "collision_incidents": 2, + "events": [ + { + "time": "...", + "event_type": "Manual_override", + "details": { + "operation_mode": "Manual", + "estop": false, + "metrics": { + "distance": 12.3, + "autonomous_distance": 12.3, + "speed": 0.4, + "battery_percentage": 78.0 + }, + "system_snapshot": { + "/gophar/system_status": { + "fault_shutdown": false, + "hv_on": true, + "battery_percentage": 78.0, + "...": "..." + }, + "/gps_base/fix": { "...": "..." }, + "odometry": { "x": 1.23, "y": 4.56, "vx": 0.4 }, + "...": "..." + } + } + } + ] +} +```` -## Node Details +Environment variables used to tag the session: -### Subscriptions +* `ROBOT_NAME` +* `FARM_NAME` +* `FIELD_NAME` +* `APPLICATION` +* `SCENARIO_NAME` -- **GPS Data** (`/gps_base/fix`): Receives and logs GPS data of the robot. -- **Odometry Data** (`/diff_drive_controller/odom`): Monitors odometry data to calculate traveled distance. -- **Battery Status** (`/diff_drive_controller/battery_status`): Logs battery level information. -- **Emergency Stop Status** (`/diff_drive_controller/estop_status`): Monitors and logs emergency stop events. -- **Hunter Status** (`/hunter_status`): Tracks the robot's current status, including operation mode and battery voltage. -- **Coordinator Task** (`/topological_navigation/execute_policy_mode/goal`): Logs tasks received from the coordinator. +--- -### Publications +## 3. Node Parameters -- **Heartbeat** (`mdbi_logger/heartbeat`): Publishes a signal every 5 seconds to indicate node activity. +All parameters are exposed through the launch file. -## License +### Core configuration -Licensed under the Apache License, Version 2.0. See the [LICENSE](http://www.apache.org/licenses/LICENSE-2.0) file for more details. +| Parameter | Type | Default | Description | +| ------------------------ | ------ | ------------- | ------------------------------------------------------ | +| `config_yaml` | string | `""` | Path to YAML config file for topics & triggers. | +| `mongodb_host` | string | `"localhost"` | Local MongoDB host. | +| `mongodb_port` | int | `27017` | Local MongoDB port. | +| `remote_mongodb_host` | string | `""` | Remote MongoDB host (empty = disabled). | +| `remote_mongodb_port` | int | `27017` | Remote MongoDB port. | +| `enable_remote_logging` | bool | `false` | Enable writes to remote MongoDB. | +| `min_distance_threshold` | double | `0.2` | Min odom step (m) required for distance update. | +| `stop_timeout` | double | `2.0` | Time (s) after last odom update before speed is set 0. | + +### Mode observer (fallback mode inference) + +Used **only** if no topic with `role: "control_mode"` is configured. + +| Parameter | Type | Default | Description | +| ------------------------------- | ------ | ------- | ------------------------------------------------------------------------ | +| `mode_observer_cmd_timeout` | double | `1.0` | Commands on `mode_observer` topic are “recent” within this many seconds. | +| `mode_observer_speed_threshold` | double | `0.01` | Robot is considered moving if speed > this (m/s). | + +Logic when active: + +* Topic with `role: "mode_observer"` (e.g. `/cmd_vel/collision_smoothed`) marks last command time. +* If odometry reports movement and: + + * cmd is recent → mode = Autonomous + * cmd is not recent → mode = Manual +* When observed mode changes from Auto → Manual: + + * `Manual_override_observed` event is logged. + * An incident is counted (MDBI). + +### Collision monitor (nav vs collision velocity) + +| Parameter | Type | Default | Description | +| -------------------------- | ------ | ------- | --------------------------------------------------------------------------------------------- | +| `collision_nav_threshold` | double | `0.01` | `/cmd_vel/nav.linear.x` must be > this to count as a forward command. | +| `collision_zero_threshold` | double | `0.001` | `/cmd_vel/collision.linear.x` considered zero if abs(x) ≤ this. | +| `collision_time_window` | double | `0.5` | Nav and collision commands must both be within this time window (s). | +| `collision_log_cooldown` | double | `1.0` | Optional legacy cooldown between collisions (can be unused if using pure falling-edge logic). | + +Collision detection (falling-edge): + +* Keep last `/cmd_vel/nav` and `/cmd_vel/collision`. +* Compute a collision **only when**: + + * Previous `/cmd_vel/collision.linear.x` had velocity (`> collision_zero_threshold`), + * Current `/cmd_vel/collision.linear.x` is ~0, + * Latest `/cmd_vel/nav.linear.x` > `collision_nav_threshold`, + * Commands are recent (`≤ collision_time_window`). +* On collision: + + * `collision_incidents` is incremented, + * A `Collision` event is logged with snapshot, + * Total collisions published on `mdbi_logger/total_collision_incidents` (Int32). + +--- + +## 4. YAML Config File + +### Top-level structure + +```yaml +git_repos: + aoc_scenario_path: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" + +topics: + - name: "/gps_base/fix" + type: "sensor_msgs/msg/NavSatFix" + log_fields: + - "latitude" + - "longitude" + - "altitude" + publish: + enable: true + topic: "mdbi_logger/gps_alt" + type: "std_msgs/msg/Float32" + field: "altitude" + + - name: "/gophar_vehicle_controller/odometry" + type: "nav_msgs/msg/Odometry" + role: "odometry" + log_fields: + - "pose.pose.position.x" + - "pose.pose.position.y" + - "twist.twist.linear.x" + publish: + enable: false + + - name: "/gophar/system_status" + type: "dynium_gophar_interfaces/msg/SystemStatus" + role: "system_status" + log_all_fields: false + log_fields: + - "fault_shutdown" + - "hv_on" + - "ignition_switch" + - "charger_interlock" + - "battery_percentage" + battery_field: "battery_percentage" + publish: + enable: true + topic: "mdbi_logger/battery_level" + type: "std_msgs/msg/Float32" + field: "battery_percentage" + intervention_on_change: + fault_shutdown: + trigger_value: true + event_type: "Fault_shutdown" + + - name: "/gophar/operation_mode" + type: "std_msgs/msg/String" + role: "control_mode" + mode_field: "data" + mode_mapping: + "AUTO": "Autonomous" + "MANUAL": "Manual" + "1": "Autonomous" + "0": "Manual" + + - name: "/cmd_vel/joy" + type: "geometry_msgs/msg/Twist" + log_fields: [] + intervention_on_message: + enable: true + event_type: "Joy_override" + + - name: "/cmd_vel/collision_smoothed" + type: "geometry_msgs/msg/Twist" + role: "mode_observer" + log_fields: [] + + - name: "/cmd_vel/nav" + type: "geometry_msgs/msg/Twist" + role: "collision_nav" + log_fields: [] + + - name: "/cmd_vel/collision" + type: "geometry_msgs/msg/Twist" + role: "collision_output" + log_fields: [] + + - name: "/gophar/steering_actuator/front" + type: "dynium_gophar_interfaces/msg/SteeringActuatorStatus" + log_all_fields: true + log_fields: [] + + - name: "/gophar/steering_actuator/rear" + type: "dynium_gophar_interfaces/msg/SteeringActuatorStatus" + log_all_fields: true + log_fields: [] + + - name: "/gophar/motor_status/motor_0" + type: "dynium_gophar_interfaces/msg/MotorStatus" + log_all_fields: true + log_fields: [] + + - name: "/gophar/motor_status/motor_1" + type: "dynium_gophar_interfaces/msg/MotorStatus" + log_all_fields: true + log_fields: [] +``` + +### Per-topic keys + +Common fields: + +* `name` + ROS topic name to subscribe to. + +* `type` + ROS message type string. Accepts: + + * `pkg/msg/MessageName` (e.g. `"sensor_msgs/msg/NavSatFix"`), or + * `pkg/MessageName` (e.g. `"sensor_msgs/NavSatFix"`). + +* `role` (optional, lowercase in logic) + Special roles: + + * `"odometry"`: used for distance/speed computation. + * `"control_mode"`: explicit operation mode (`Autonomous` vs `Manual`). + * `"estop"`: emergency stop; used to generate `EMS` incidents. + * `"system_status"`: can carry `battery_field` and fault triggers. + * `"mode_observer"`: used by mode observer (e.g. `/cmd_vel/collision_smoothed`). + * `"collision_nav"`: nav velocity command. + * `"collision_output"`: collision-limited velocity command. + +* `log_fields` (list of dotted paths) + Fields to extract into `system_snapshot[topic_name]`. + Only used if `log_all_fields` is `false`. + +* `log_all_fields` (bool, default: false) + If `true`, the full message is converted to a dict and stored in `system_snapshot[topic_name]`. + Ignores `log_fields`. + +* `battery_field` (optional, string) + Field name under this topic that carries battery percentage; updates `current_battery`. + +#### Dynamic republishing + +```yaml +publish: + enable: true + topic: "mdbi_logger/battery_level" + type: "std_msgs/msg/Float32" + field: "battery_percentage" +``` + +* `enable`: `true`/`false` +* `topic`: output topic name +* `type`: output message type +* `field`: dotted field in input message; mapped to `.data` of the output (with type casting). + +#### Control mode + +For topics with `role: "control_mode"`: + +* `mode_field` (string) + Dotted field path inside message holding the raw mode value (e.g. `"data"`). + +* `mode_mapping` (dict) + Map raw values to `"Autonomous"` or `"Manual"` + Example: `"AUTO" → "Autonomous"`, `"0" → "Manual"`. + If unmapped and `str(value) == "3"`, forced to Manual; otherwise treated as Autonomous. + +Changing from `Autonomous` to `Manual`: + +* Ends an autonomy segment (time tracking). +* Logs `Manual_override` event. +* Increments `incidents` (MDBI) even if now in Manual (forced by `force_count=True`). + +#### E-stop role + +For `role: "estop"` topics, additional config: + +* `field` (optional, default `"data"`) + Boolean field to check. + When it changes and becomes `True`: + + * `EMS` intervention is logged. + * Incidents increment only if mode = Autonomous. + +#### YAML-driven interventions + +Per-topic: + +* `intervention_on_message`: + + ```yaml + intervention_on_message: + enable: true + event_type: "Joy_override" + ``` + + If enabled, **every message** on that topic triggers an event of this type via `trigger_intervention`. + +* `intervention_on_change`: + + ```yaml + intervention_on_change: + fault_shutdown: + trigger_value: true + event_type: "Fault_shutdown" + ``` + + * Key (`fault_shutdown`) is a field name in `data`. + * `trigger_value` optional; if present, event fires only when field changes **and** equals `trigger_value`. + * `event_type` is the event name stored in DB. + +--- + +## 5. Mode Handling and Incidents + +* Default mode at startup is set to `Autonomous`: + + ```python + self.AUTO = 'Autonomous' + self.MAN = 'Manual' + self.details = {'estop': False, 'operation_mode': self.AUTO} + ``` + +* `incidents` are only incremented when: + + * `trigger_intervention(...)` is called and: + + * `operation_mode == "Autonomous"`, or + * `force_count=True` (for explicit/observed manual overrides). + +* E-stop, faults, joy overrides, etc. in **Manual** mode: + + * Are logged to DB (event + snapshot), + * Do **not** increment `incidents`, thus do not affect MDBI. + +--- + +## 6. Launching the Node + +Example launch file (simplified; adapt package/executable names): + +```python +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + config_yaml_arg = DeclareLaunchArgument( + 'config_yaml', + default_value='', + description='Path to AutonomyMetricsLogger YAML config', + ) + + # ... declare other args (mongodb_host, ports, thresholds, etc.) ... + + config_yaml = LaunchConfiguration('config_yaml') + + metrics_logger_node = Node( + package='autonomy_metrics', # adjust + executable='metric_logger', # adjust + name='mdbi_logger_dynamic', + output='screen', + parameters=[{ + 'config_yaml': config_yaml, + 'mongodb_host': LaunchConfiguration('mongodb_host'), + 'mongodb_port': LaunchConfiguration('mongodb_port'), + 'remote_mongodb_host': LaunchConfiguration('remote_mongodb_host'), + 'remote_mongodb_port': LaunchConfiguration('remote_mongodb_port'), + 'enable_remote_logging': LaunchConfiguration('enable_remote_logging'), + 'min_distance_threshold': LaunchConfiguration('min_distance_threshold'), + 'stop_timeout': LaunchConfiguration('stop_timeout'), + 'mode_observer_cmd_timeout': LaunchConfiguration('mode_observer_cmd_timeout'), + 'mode_observer_speed_threshold': LaunchConfiguration('mode_observer_speed_threshold'), + 'collision_nav_threshold': LaunchConfiguration('collision_nav_threshold'), + 'collision_zero_threshold': LaunchConfiguration('collision_zero_threshold'), + 'collision_time_window': LaunchConfiguration('collision_time_window'), + 'collision_log_cooldown': LaunchConfiguration('collision_log_cooldown'), + }], + ) + + return LaunchDescription([ + config_yaml_arg, + # ... other DeclareLaunchArgument ... + metrics_logger_node, + ]) +``` + +### Example launch usage + +Minimal: + +```bash +ros2 launch autonomy_metrics_logger autonomy_metrics_logger.launch.py \ + config_yaml:=/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/autonomy_metrics_logger/config/metrics_gophar.yaml +``` + +With remote DB + tuned thresholds: + +```bash +ros2 launch autonomy_metrics_logger autonomy_metrics_logger.launch.py \ + config_yaml:=/path/to/metrics_gophar.yaml \ + mongodb_host:=localhost \ + mongodb_port:=27017 \ + enable_remote_logging:=true \ + remote_mongodb_host:=10.0.0.42 \ + remote_mongodb_port:=27017 \ + min_distance_threshold:=0.1 \ + stop_timeout:=1.5 \ + mode_observer_cmd_timeout:=0.8 \ + mode_observer_speed_threshold:=0.02 \ + collision_nav_threshold:=0.02 \ + collision_zero_threshold:=0.001 \ + collision_time_window:=0.4 +``` + +--- + +## 7. Testing Tips + +* Verify topics: + + ```bash + ros2 topic list + ros2 topic info /gophar_vehicle_controller/odometry + ros2 topic info /gophar/system_status + ``` + +* Trigger joystick interventions: + + ```bash + ros2 topic pub --once /cmd_vel/joy geometry_msgs/msg/Twist \ + "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" + ``` + +* Check DB content (Mongo shell or Mongo client) in `robot_incidents.sessions`. + +The README covers: + +* All node parameters, +* YAML configuration options, +* How incidents, MDBI, collisions, and snapshots are computed, +* How to launch and test the system. diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index 9eee832..edc41a2 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -2,7 +2,6 @@ """ YAML-driven AutonomyMetricsLogger Author: Ibrahim Hroob - JABASAI -Updated: Fixed Battery Pub & Added Intervention Snapshot """ import os @@ -19,7 +18,7 @@ from rclpy.time import Time # Keep a few explicit imports used by default publishers (std msgs) -from std_msgs.msg import Bool, Float32, Int32, String +from std_msgs.msg import Bool, Float32, Int32 from autonomy_metrics.db_mgr import DatabaseMgr as DBMgr @@ -227,7 +226,8 @@ def __init__(self): try: git_cfg = self.config.get('git_repos', {}) for label, p in git_cfg.items(): - git_repos.append({label: self.get_git_info(p)}) + repo_path = os.path.join('/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario', p) + git_repos.append({label: self.get_git_info(repo_path)}) except Exception: pass @@ -678,9 +678,6 @@ def collision_nav_callback(self, topic_name, msg): self.system_snapshot['cmd_vel_nav'] = { 'linear_x': msg.linear.x, 'linear_y': msg.linear.y, - 'linear_z': msg.linear.z, - 'angular_x': msg.angular.x, - 'angular_y': msg.angular.y, 'angular_z': msg.angular.z, } # IMPORTANT: do NOT call check_collision_condition here @@ -703,9 +700,6 @@ def collision_output_callback(self, topic_name, msg): self.system_snapshot['cmd_vel_collision'] = { 'linear_x': msg.linear.x, 'linear_y': msg.linear.y, - 'linear_z': msg.linear.z, - 'angular_x': msg.angular.x, - 'angular_y': msg.angular.y, 'angular_z': msg.angular.z, } @@ -798,8 +792,7 @@ def update_db_metrics(self): dbm.update_incidents(self.incidents) dbm.update_autonomous_distance(self.autonomous_distance) dbm.update_mdbi(mdbi_val) - if hasattr(dbm, "update_collision_incidents"): - dbm.update_collision_incidents(self.collision_incidents) + dbm.update_collision_incidents(self.collision_incidents) except Exception as e: self.get_logger().warn(f"DB update failed: {e}") @@ -847,11 +840,8 @@ def log_collision_event(self): "linear": { "x": float(self.last_nav_cmd.linear.x), "y": float(self.last_nav_cmd.linear.y), - "z": float(self.last_nav_cmd.linear.z), }, "angular": { - "x": float(self.last_nav_cmd.angular.x), - "y": float(self.last_nav_cmd.angular.y), "z": float(self.last_nav_cmd.angular.z), }, }, @@ -859,11 +849,8 @@ def log_collision_event(self): "linear": { "x": float(self.last_collision_cmd.linear.x), "y": float(self.last_collision_cmd.linear.y), - "z": float(self.last_collision_cmd.linear.z), }, "angular": { - "x": float(self.last_collision_cmd.angular.x), - "y": float(self.last_collision_cmd.angular.y), "z": float(self.last_collision_cmd.angular.z), }, }, diff --git a/src/autonomy_metrics/config/metrics_full.yaml b/src/autonomy_metrics/config/metrics_full.yaml index c6e0e0c..c210be9 100644 --- a/src/autonomy_metrics/config/metrics_full.yaml +++ b/src/autonomy_metrics/config/metrics_full.yaml @@ -1,16 +1,17 @@ git_repos: - scenario: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario" - autonomy_metrics_logger: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/autonomy_metrics_logger" - navfn_planner: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/navfn_planner" - perception_boundary_estimator: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/perception_boundary_estimator" - pointcloud_processing: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/pointcloud_processing" - robot_boundary_checker: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/robot_boundary_checker" - ros2_topic_monitor: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/ros2_topic_monitor" - row_nav_planner: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/jabas/row_nav_planner" - ppl_gofar_platform: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/robots/ppl_gofar_platform" - topological_navigation: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/contrib/topological_navigation" - sentor: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/contrib/sentor" - kiss-icp: "/home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario/contrib/kiss-icp" + # all repos are with reference to /home/ros/aoc_strawberry_scenario_ws/src/aoc_strawberry_scenario + scenario: "." + autonomy_metrics_logger: "jabas/autonomy_metrics_logger" + navfn_planner: "jabas/navfn_planner" + perception_boundary_estimator: "jabas/perception_boundary_estimator" + pointcloud_processing: "jabas/pointcloud_processing" + robot_boundary_checker: "jabas/robot_boundary_checker" + ros2_topic_monitor: "jabas/ros2_topic_monitor" + row_nav_planner: "jabas/row_nav_planner" + ppl_gofar_platform: "robots/ppl_gofar_platform" + topological_navigation: "contrib/topological_navigation" + sentor: "contrib/sentor" + kiss-icp: "contrib/kiss-icp" topics: # --------------------------------------------------------- @@ -18,15 +19,7 @@ topics: # --------------------------------------------------------- - name: "/gps_base/fix" type: "sensor_msgs/msg/NavSatFix" - log_fields: - - "latitude" - - "longitude" - - "altitude" - publish: - enable: false - topic: "mdbi_logger/gps_alt" - type: "std_msgs/msg/Float32" - field: "altitude" + log_all_fields: true # --------------------------------------------------------- # Odometry @@ -47,13 +40,7 @@ topics: - name: "/gophar/system_status" type: "dynium_gophar_interfaces/msg/SystemStatus" role: "system_status" - log_fields: [] - log_fields: - - "fault_shutdown" - - "hv_on" - - "ignition_switch" - - "charger_interlock" - - "battery_percentage" + log_all_fields: true # Tell logger which field carries battery level so it can be attached battery_field: "battery_percentage" @@ -87,7 +74,7 @@ topics: # "0": "Manual" # --------------------------------------------------------- - # X. Mode observation via velocity command + # 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" @@ -110,7 +97,6 @@ topics: # --------------------------------------------------------- # Collision monitor: nav vs collision-filtered velocity # --------------------------------------------------------- - - name: "/cmd_vel/nav" type: "geometry_msgs/msg/Twist" role: "collision_nav" @@ -124,23 +110,18 @@ topics: # --------------------------------------------------------- # Steering actuator + motor status snapshots for interventions # --------------------------------------------------------- - - name: "/gophar/steering_actuator/front" type: "dynium_gophar_interfaces/msg/SteeringActuatorStatus" - log_all_fields: true # <--- NEW - log_fields: [] # optional, ignored if log_all_fields is true + log_all_fields: true - name: "/gophar/steering_actuator/rear" type: "dynium_gophar_interfaces/msg/SteeringActuatorStatus" - log_all_fields: true # <--- NEW - log_fields: [] # optional, ignored if log_all_fields is true + log_all_fields: true - name: "/gophar/motor_status/motor_0" type: "dynium_gophar_interfaces/msg/MotorStatus" - log_all_fields: true # <--- NEW - log_fields: [] # optional, ignored if log_all_fields is true + log_all_fields: true - name: "/gophar/motor_status/motor_1" type: "dynium_gophar_interfaces/msg/MotorStatus" - log_all_fields: true # <--- NEW - log_fields: [] # optional, ignored if log_all_fields is true \ No newline at end of file + log_all_fields: true \ No newline at end of file diff --git a/src/autonomy_metrics/launch/autonomy_metrics.launch.py b/src/autonomy_metrics/launch/autonomy_metrics.launch.py index bb7aec9..91f5984 100644 --- a/src/autonomy_metrics/launch/autonomy_metrics.launch.py +++ b/src/autonomy_metrics/launch/autonomy_metrics.launch.py @@ -1,30 +1,167 @@ +#!/usr/bin/env python3 + import os from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from ament_index_python.packages import get_package_share_directory def generate_launch_description(): + # ------------------------------------------------------------------------- + # 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 YAML configuration file for mdbi_logger' + default_value=os.path.join(pkg_dir, 'config', 'metrics_full.yaml'), + description='Path to the YAML config file for AutonomyMetricsLogger', ) - node = Node( + 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', # ensure your setup/install maps executable correctly - name='mdbi_logger', + executable='metric_logger', + name='mdbi_logger_dynamic', output='screen', parameters=[{ - 'config_yaml': LaunchConfiguration('config_yaml'), - # optional override - 'mongodb_host': 'localhost', - 'mongodb_port': 27018, - }] + '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, + }], ) - return LaunchDescription([config_yaml_arg, node]) + 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, + ]) From ca0598e8f34e087f0c0ed3d45695a9a646653639 Mon Sep 17 00:00:00 2001 From: ibrahim Date: Thu, 11 Dec 2025 19:36:58 +0000 Subject: [PATCH 9/9] Enhance AutonomyMetricsLogger with improved logging for configuration, intervention triggers, and collision detection --- .../autonomy_metrics/metric_logger.py | 225 +++++++++++++----- 1 file changed, 163 insertions(+), 62 deletions(-) diff --git a/src/autonomy_metrics/autonomy_metrics/metric_logger.py b/src/autonomy_metrics/autonomy_metrics/metric_logger.py index edc41a2..d6573a6 100644 --- a/src/autonomy_metrics/autonomy_metrics/metric_logger.py +++ b/src/autonomy_metrics/autonomy_metrics/metric_logger.py @@ -79,6 +79,7 @@ def import_msg_type(type_str: str): 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 @@ -93,6 +94,7 @@ def get_nested_field(obj, path: str): except Exception as e: raise AttributeError(f"Failed to get '{path}': {e}") + class AutonomyMetricsLogger(Node): def __init__(self): super().__init__('mdbi_logger_dynamic') @@ -105,9 +107,9 @@ def __init__(self): 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('enable_remote_logging', False) self.declare_parameter('min_distance_threshold', 0.2) - self.declare_parameter('stop_timeout', 2.0) + 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 @@ -132,6 +134,10 @@ def __init__(self): self.stop_timeout = self.get_parameter('stop_timeout').get_parameter_value().double_value self.get_logger().info(f"Config path: {self.config_path}") + self.get_logger().info( + f"Collision params: nav_thr={self.collision_nav_threshold}, " + f"zero_thr={self.collision_zero_threshold}, time_window={self.collision_time_window}" + ) # DB Managers self.db_mgr_local = DBMgr(host=self.mongo_host, port=self.mongo_port) @@ -139,6 +145,9 @@ def __init__(self): 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}") @@ -151,8 +160,8 @@ def __init__(self): self.autonomous_start_time = None self.AUTO = 'Autonomous' self.MAN = 'Manual' - self.details = {'estop': False, 'operation_mode': self.AUTO} - + 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 @@ -175,13 +184,13 @@ def __init__(self): # Stores the latest extracted values from all topics # ---------------------------------------------------- self.system_snapshot = {} - + # Snapshot of latest values per field for change detection self.prev_field_values = {} # Current battery level (updated from any topic with 'battery_field') - self.current_battery = None - + self.current_battery = None + # Odometry / Speed state self.previous_x = None self.previous_y = None @@ -220,46 +229,64 @@ def __init__(self): 'application': os.getenv('APPLICATION', 'UNDEFINED'), 'scenario_name': os.getenv('SCENARIO_NAME', 'UNDEFINED'), } - + + 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']}" + ) + 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) - git_repos.append({label: self.get_git_info(repo_path)}) - except Exception: - pass + 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 ) - + 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: continue + + if not name or not type_str: + self.get_logger().warn(f"Skipping invalid topic config: {item}") + continue self.topic_cfg_map[name] = item @@ -268,6 +295,7 @@ def load_and_setup_config(self): 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', {}) @@ -276,7 +304,10 @@ def load_and_setup_config(self): 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 Pub: {pub_cfg['topic']} -> {name}") + 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 @@ -303,12 +334,11 @@ def load_and_setup_config(self): 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})") + 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. @@ -339,13 +369,17 @@ def trigger_intervention(self, event_type: str, extra: dict | None = None, force if count_incident: self.incidents += 1 + self.get_logger().info( + f"[Intervention] type={event_type}, mode={current_mode}, " + f"counted={count_incident}, total_incidents={self.incidents}" + ) + base_details = dict(self.details) # estop, operation_mode, etc. base_details.update(extra) base_details["system_snapshot"] = self.system_snapshot.copy() self.log_event(event_type, base_details) - def handle_intervention_triggers(self, topic_name: str, data: dict, cfg: dict): """ Apply YAML-configured triggers for this topic. @@ -356,6 +390,9 @@ def handle_intervention_triggers(self, topic_name: str, data: dict, cfg: dict): msg_trig = cfg.get("intervention_on_message", {}) if msg_trig.get("enable", False): evt_type = msg_trig.get("event_type", f"{topic_name}_activity") + self.get_logger().info( + f"[Trigger] intervention_on_message on topic '{topic_name}' -> event '{evt_type}'" + ) self.trigger_intervention(evt_type, extra={"topic": topic_name}) # 2) Trigger on change of specific fields @@ -387,6 +424,10 @@ def handle_intervention_triggers(self, topic_name: str, data: dict, cfg: dict): "new_value": new, "prev_value": prev, } + self.get_logger().info( + f"[Trigger] field_change on '{topic_name}.{field_name}' " + f"from {prev} -> {new} -> event '{evt_type}'" + ) self.trigger_intervention(evt_type, extra=extra) # ------------------------- @@ -412,6 +453,9 @@ def odom_role_callback(self, topic_name, msg): self.previous_y = pos.y self.previous_time = current_time self.last_odom_update_time = current_time + self.get_logger().info( + f"[Odom] Initial pose set x={pos.x:.3f}, y={pos.y:.3f}" + ) return dx = pos.x - self.previous_x @@ -419,6 +463,7 @@ def odom_role_callback(self, topic_name, msg): 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'): @@ -448,11 +493,16 @@ def odom_role_callback(self, topic_name, msg): 'speed': self.speed, } + self.get_logger().debug( + f"[Odom] step_dist={dist:.3f} m, total_dist={self.distance:.3f} m, " + f"auto_dist={self.autonomous_distance:.3f} m, speed={self.speed:.3f} m/s, " + f"mode={self.details.get('operation_mode')}" + ) + self.publish_distance(self.distance) self.publish_speed(self.speed) self.handle_dynamic_publish(topic_name, msg) - def control_mode_role_callback(self, topic_name, msg): cfg = self.topic_cfg_map.get(topic_name, {}) mode_field = cfg.get('mode_field', 'data') @@ -475,7 +525,9 @@ def control_mode_role_callback(self, topic_name, msg): 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: {prev_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: @@ -494,7 +546,6 @@ def control_mode_role_callback(self, topic_name, msg): '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') @@ -507,12 +558,11 @@ def estop_role_callback(self, topic_name, msg): if v != self.details.get('estop', False): self.details['estop'] = v - self.get_logger().info(f"E-Stop: {v}") + self.get_logger().info(f"E-Stop state changed: {v}") if v: # E-stop counts as intervention self.trigger_intervention('EMS') - def generic_callback(self, topic_name, msg, log_fields): cfg = self.topic_cfg_map.get(topic_name, {}) role = cfg.get('role', '').lower() @@ -523,12 +573,15 @@ def generic_callback(self, topic_name, msg, log_fields): self.system_snapshot['mode_observer'] = { 'topic': topic_name } + self.get_logger().debug( + f"[ModeObserver] Received command on {topic_name}, " + f"marking recent autonomous command." + ) data = {} log_all = bool(cfg.get("log_all_fields", False)) if log_all: - # NEW: capture the entire message into the snapshot try: data = ros_msg_to_dict(msg) except Exception as e: @@ -537,7 +590,6 @@ def generic_callback(self, topic_name, msg, log_fields): ) data = {} else: - # Original behaviour: only selected log_fields for f in log_fields: try: data[f] = get_nested_field(msg, f) @@ -551,11 +603,13 @@ def generic_callback(self, topic_name, msg, log_fields): # 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] - - # IMPORTANT: - # No per-message DB logging anymore. - # We only call log_event(...) on interventions / collisions / mode changes. + if old_batt is None or abs(self.current_battery - old_batt) >= 1.0: + self.get_logger().info( + f"[Battery] {topic_name}.{battery_field} updated: " + f"{old_batt} -> {self.current_battery}" + ) # Apply YAML-configured intervention triggers (message & field-change) self.handle_intervention_triggers(topic_name, data, cfg) @@ -563,22 +617,17 @@ def generic_callback(self, topic_name, msg, log_fields): # 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 + 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'): - # --------------------------------------------------------- - # 4. FIX: Robust Type Casting - # If target is Float32 but val is int, simple assignment works - # but explicit cast is safer. - # --------------------------------------------------------- if 'Float' in pub_msg_cls.__name__: out_msg.data = float(val) elif 'Int' in pub_msg_cls.__name__: @@ -594,18 +643,18 @@ def handle_dynamic_publish(self, topic_name, msg): try: setattr(out_msg, attr, val) break - except: continue - + except Exception: + continue + publisher.publish(out_msg) - + self.get_logger().debug( + f"[DynamicPublish] topic={topic_name} -> republished " + f"value={val} ({pub_msg_cls.__name__})" + ) + except Exception as e: - # --------------------------------------------------------- - # 5. FIX: Log Errors - # Don't fail silently. Tell user why it failed. - # --------------------------------------------------------- self.get_logger().error(f"Dynamic publish failed for {topic_name}: {e}") - def update_observed_mode_from_odom(self, is_moving: bool, now=None): """ Fallback mode inference when there is no explicit /gophar/operation_mode. @@ -630,6 +679,11 @@ def update_observed_mode_from_odom(self, is_moving: bool, now=None): dt = (now - self.last_auto_cmd_time).nanoseconds * 1e-9 auto_cmd_recent = dt <= self.mode_observer_cmd_timeout + self.get_logger().debug( + f"[ModeObserver] is_moving={is_moving}, auto_cmd_recent={auto_cmd_recent}, " + f"prev_mode={prev_mode}" + ) + if not is_moving: # Don't flip modes just because we stopped; keep previous return @@ -658,7 +712,6 @@ def update_observed_mode_from_odom(self, is_moving: bool, now=None): "mode_source": "observed", "observer_topic": self.mode_observer_topic_name, } - # Uses the same helper we defined earlier self.trigger_intervention("Manual_override_observed", extra=extra, force_count=True) elif prev_mode != self.AUTO and new_mode == self.AUTO: @@ -666,7 +719,6 @@ def update_observed_mode_from_odom(self, is_moving: bool, now=None): self.details['operation_mode'] = new_mode - def collision_nav_callback(self, topic_name, msg): """ Store latest /cmd_vel/nav command. @@ -680,8 +732,12 @@ def collision_nav_callback(self, topic_name, msg): 'linear_y': msg.linear.y, 'angular_z': msg.angular.z, } - # IMPORTANT: do NOT call check_collision_condition here + 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): """ @@ -703,10 +759,14 @@ def collision_output_callback(self, topic_name, msg): '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: @@ -726,7 +786,9 @@ def check_collision_condition(self, now, coll_vx_now: float): """ # Need a nav command to interpret the collision behaviour if self.last_nav_cmd is None or self.last_nav_time is None: - # Still update state, but cannot decide yet + self.get_logger().debug( + "[Collision] No NAV cmd yet; cannot evaluate collision condition." + ) collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold self.collision_prev_has_velocity = collision_has_velocity_now return @@ -734,6 +796,10 @@ def check_collision_condition(self, now, coll_vx_now: float): # Check nav recency dt_nav = (now - self.last_nav_time).nanoseconds * 1e-9 if dt_nav > self.collision_time_window: + self.get_logger().debug( + f"[Collision] NAV cmd too old ({dt_nav:.3f}s > {self.collision_time_window}s); " + "skipping collision evaluation." + ) collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold self.collision_prev_has_velocity = collision_has_velocity_now return @@ -742,6 +808,10 @@ def check_collision_condition(self, now, coll_vx_now: float): # Does nav actually request motion? if nav_vx <= self.collision_nav_threshold: + self.get_logger().debug( + f"[Collision] NAV vx={nav_vx:.3f} <= nav_threshold={self.collision_nav_threshold}; " + "no forward motion requested." + ) collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold self.collision_prev_has_velocity = collision_has_velocity_now return @@ -749,16 +819,25 @@ def check_collision_condition(self, now, coll_vx_now: float): # Edge detection on /cmd_vel/collision collision_has_velocity_now = abs(coll_vx_now) > self.collision_zero_threshold + self.get_logger().debug( + f"[Collision] prev_has_vel={self.collision_prev_has_velocity}, " + f"now_has_vel={collision_has_velocity_now}, coll_vx_now={coll_vx_now:.3f}, " + f"nav_vx={nav_vx:.3f}" + ) + # Falling edge: had velocity before, now ~zero if self.collision_prev_has_velocity and not collision_has_velocity_now: # This is one collision incident self.collision_incidents += 1 + self.get_logger().info( + f"[Collision] Detected collision incident #{self.collision_incidents} " + f"(nav_vx={nav_vx:.3f}, coll_vx_now={coll_vx_now:.3f})" + ) self.log_collision_event() # Update for next call self.collision_prev_has_velocity = collision_has_velocity_now - # ------------------------- # Timer & Logging # ------------------------- @@ -769,8 +848,12 @@ def timer_callback(self): now = self.get_clock().now() time_since_move = (now - self.last_odom_update_time).nanoseconds * 1e-9 - + if time_since_move > self.stop_timeout and self.speed > 0.0: + self.get_logger().info( + f"[Timer] Robot has not moved for {time_since_move:.2f}s; " + "forcing speed to 0.0" + ) self.speed = 0.0 self.publish_speed(0.0) @@ -782,6 +865,11 @@ def update_db_metrics(self): # No incidents → use autonomous distance as-is (no division) mdbi_val = float(self.autonomous_distance) + 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) @@ -796,7 +884,6 @@ def update_db_metrics(self): 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 = {} @@ -810,12 +897,17 @@ def log_event(self, msg='', details=None): event_time = datetime.now(tz=timezone.utc) event = {'time': event_time, 'event_type': msg, 'details': details} + self.get_logger().info( + f"[Event] type={msg}, time={event_time.isoformat()}, " + f"incidents={self.incidents}, collisions={self.collision_incidents}" + ) + self.db_mgr_local.add_event(event) if self.db_mgr_remote: try: self.db_mgr_remote.add_event(event) - except Exception: - pass + except Exception as e: + self.get_logger().warn(f"Remote DB event log failed: {e}") incidents_msg = Int32() incidents_msg.data = self.incidents @@ -823,7 +915,6 @@ def log_event(self, msg='', details=None): self.update_db_metrics() - def log_collision_event(self): """ Log a 'Collision' event, capturing: @@ -858,6 +949,11 @@ def log_collision_event(self): "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) @@ -865,18 +961,19 @@ def log_collision_event(self): 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): """ @@ -954,9 +1051,7 @@ def git(args): # NEW: Last commit message (full body or just subject) try: - # Subject only: info["commit_message"] = git(["log", "-1", "--pretty=%s"]) - # If you prefer full body, use: "--pretty=%B" except Exception: info["commit_message"] = None @@ -995,10 +1090,16 @@ def main(args=None): try: rclpy.spin(node) except KeyboardInterrupt: + node.get_logger().info("Shutting down AutonomyMetricsLogger (KeyboardInterrupt).") pass finally: + node.get_logger().info( + f"Final stats: distance={node.distance:.3f}, auto_distance={node.autonomous_distance:.3f}, " + f"incidents={node.incidents}, collisions={node.collision_incidents}" + ) node.destroy_node() rclpy.shutdown() + if __name__ == '__main__': main() \ No newline at end of file