From d581ca8fadad4aa2e2fc42d9bb45484b5bc1a603 Mon Sep 17 00:00:00 2001 From: Praveen Kalva Date: Wed, 14 May 2025 20:24:38 +0000 Subject: [PATCH 1/2] added collision logger component --- .../knowledge/defaults/computation_graph.yaml | 5 +- GEMstack/onboard/execution/entrypoint.py | 10 ++ .../onboard/other/gazebo_collision_logger.py | 108 ++++++++++++++++++ launch/fixed_route.yaml | 4 +- launch/gazebo_simulation.yaml | 1 + 5 files changed, 125 insertions(+), 3 deletions(-) create mode 100644 GEMstack/onboard/other/gazebo_collision_logger.py diff --git a/GEMstack/knowledge/defaults/computation_graph.yaml b/GEMstack/knowledge/defaults/computation_graph.yaml index aa79c7cbc..dfb362cde 100644 --- a/GEMstack/knowledge/defaults/computation_graph.yaml +++ b/GEMstack/knowledge/defaults/computation_graph.yaml @@ -69,7 +69,10 @@ components: outputs: trajectory - trajectory_tracking: inputs: [vehicle, trajectory] - outputs: + outputs: - signaling: inputs: [intent] + outputs: + - collision_logger: + inputs: [] outputs: \ No newline at end of file diff --git a/GEMstack/onboard/execution/entrypoint.py b/GEMstack/onboard/execution/entrypoint.py index b1e7a8d10..b2f225c1e 100644 --- a/GEMstack/onboard/execution/entrypoint.py +++ b/GEMstack/onboard/execution/entrypoint.py @@ -118,6 +118,16 @@ def caution_callback(k,variant): if v is None: continue other_components[k] = mission_executor.make_component(v,k,'GEMstack.onboard.other', {'vehicle_interface':vehicle_interface}) + # Add collision logger to other_components if in gazebo simulation and collision logging is enabled + if mode == 'simulation' and settings.get('run.collision_logging', False) and name == 'drive': + from ..other.gazebo_collision_logger import GazeboCollisionLogger + other_components['collision_logger'] = mission_executor.make_component( + {'type': 'GazeboCollisionLogger', 'module': 'gazebo_collision_logger'}, + 'collision_logger', + 'GEMstack.onboard.other', + {'vehicle_interface': vehicle_interface} + ) + mission_executor.add_pipeline(name,perception_components,planning_components,other_components) #configure logging diff --git a/GEMstack/onboard/other/gazebo_collision_logger.py b/GEMstack/onboard/other/gazebo_collision_logger.py new file mode 100644 index 000000000..cb8ff8f3f --- /dev/null +++ b/GEMstack/onboard/other/gazebo_collision_logger.py @@ -0,0 +1,108 @@ +from ...utils import settings +from ..component import Component +import rospy +from gazebo_msgs.msg import ContactsState +from datetime import datetime +import os + +class GazeboCollisionLogger(Component): + """Component that logs collision data from Gazebo simulation.""" + + def __init__(self, vehicle_interface): + super().__init__() + self.vehicle_interface = vehicle_interface + self.log_file = None + self.initialized = False + + def initialize(self): + """Initialize the collision logger component.""" + if not self.initialized: + # Get log folder from settings + log_settings = settings.get('run.log', {}) + topfolder = log_settings.get('folder', 'logs') + prefix = log_settings.get('prefix', '') + suffix = log_settings.get('suffix', datetime.now().strftime('%Y-%m-%d_%H-%M-%S')) + logfolder = os.path.join(topfolder, prefix + suffix) + + # Create collision log file + self.log_file = os.path.join(logfolder, 'collision_log.txt') + os.makedirs(os.path.dirname(self.log_file), exist_ok=True) + + # Subscribe to contact sensor topic + rospy.Subscriber('/contact_sensor', ContactsState, self.contact_callback) + + self.initialized = True + rospy.loginfo("Collision logger initialized. Logging to: %s", self.log_file) + rospy.loginfo("Waiting for collision events...") + + def simplify_collision_name(self, name): + """Simplify collision names to be more readable.""" + if "base_link" in name: + return "vehicle body" + elif "front_rack" in name: + return "vehicle front bumper" + elif "rear_rack" in name: + return "vehicle rear bumper" + elif "::" in name: + return name.split("::")[0] + return name + + def contact_callback(self, msg): + """Callback for processing collision messages.""" + if not msg.states: # If no contacts + return + + timestamp = datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3] + + # Create a dictionary to store unique collisions + unique_collisions = {} + + # Process all collision states + for state in msg.states: + collision_key = tuple(sorted([state.collision1_name, state.collision2_name])) + if collision_key not in unique_collisions or state.depths[0] > unique_collisions[collision_key].depths[0]: + unique_collisions[collision_key] = state + + # Log collision information + rospy.loginfo("\n=== Collision Detected at %s ===", timestamp) + + with open(self.log_file, 'a') as f: + f.write(f"\n=== Collision Detected at {timestamp} ===\n") + + for i, (collision_key, state) in enumerate(unique_collisions.items(), 1): + collision1 = self.simplify_collision_name(state.collision1_name) + collision2 = self.simplify_collision_name(state.collision2_name) + + # Reorder collisions to put vehicle parts first + if "vehicle" in collision2: + collision1, collision2 = collision2, collision1 + + # Log collision details + rospy.loginfo("\nContact %d:", i) + rospy.loginfo("Collision between: %s and %s", collision1, collision2) + + pos = state.contact_positions[0] + pos_str = f"Contact Position: x={pos.x:.3f}, y={pos.y:.3f}, z={pos.z:.3f}" + rospy.loginfo(pos_str) + + normal = state.contact_normals[0] + normal_str = f"Contact Normal: x={normal.x:.3f}, y={normal.y:.3f}, z={normal.z:.3f}" + rospy.loginfo(normal_str) + + depth_str = f"Contact Depth: {state.depths[0]:.3f}" + rospy.loginfo(depth_str) + + # Write to file + f.write(f"\nContact {i}:\n") + f.write(f"Collision between: {collision1} and {collision2}\n") + f.write(f"{pos_str}\n") + f.write(f"{normal_str}\n") + f.write(f"{depth_str}\n") + f.write("-" * 50 + "\n") + + f.write("\n") + rospy.loginfo("-" * 50) + + def update(self): + """Update method required by Component base class.""" + pass # All work is done in the ROS callback \ No newline at end of file diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 58d83b43b..368cb5b9d 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -66,8 +66,7 @@ variants: vehicle_interface: type: gem_simulator.GEMDoubleIntegratorSimulationInterface args: - scene: !relative_path '../scenes/xyhead_demo.yaml' - + scene: !relative_path '../scenes/xyhead_demo.yaml' drive: perception: state_estimation : OmniscientStateEstimator @@ -79,6 +78,7 @@ variants: mode: simulation vehicle_interface: type: gem_gazebo.GEMGazeboInterface + collision_logging: True drive: perception: state_estimation: GNSSStateEstimator # Matches your Gazebo GNSS implementation diff --git a/launch/gazebo_simulation.yaml b/launch/gazebo_simulation.yaml index be77df511..cfa33f7a2 100644 --- a/launch/gazebo_simulation.yaml +++ b/launch/gazebo_simulation.yaml @@ -68,6 +68,7 @@ variants: type: gem_gazebo.GEMGazeboInterface # args: # scene: !relative_path '../scenes/gazebo.yaml' + collision_logging: True # Enable collision logging for gazebo simulation drive: perception: From c069d1ea46f7261c8a41332dd04bf9e2b33e58f4 Mon Sep 17 00:00:00 2001 From: Praveen Kalva Date: Wed, 14 May 2025 22:04:47 +0000 Subject: [PATCH 2/2] updated collision logger to use gemstacks logger --- .../knowledge/defaults/computation_graph.yaml | 4 +- GEMstack/onboard/execution/entrypoint.py | 8 +- .../onboard/other/gazebo_collision_logger.py | 106 ++++++------------ docs/Gazebo Simulation Documentation.md | 14 +++ 4 files changed, 59 insertions(+), 73 deletions(-) diff --git a/GEMstack/knowledge/defaults/computation_graph.yaml b/GEMstack/knowledge/defaults/computation_graph.yaml index dfb362cde..505b85684 100644 --- a/GEMstack/knowledge/defaults/computation_graph.yaml +++ b/GEMstack/knowledge/defaults/computation_graph.yaml @@ -73,6 +73,6 @@ components: - signaling: inputs: [intent] outputs: - - collision_logger: + - gazebo_collision_logger: inputs: [] - outputs: \ No newline at end of file + outputs: \ No newline at end of file diff --git a/GEMstack/onboard/execution/entrypoint.py b/GEMstack/onboard/execution/entrypoint.py index b2f225c1e..849450e15 100644 --- a/GEMstack/onboard/execution/entrypoint.py +++ b/GEMstack/onboard/execution/entrypoint.py @@ -121,9 +121,9 @@ def caution_callback(k,variant): # Add collision logger to other_components if in gazebo simulation and collision logging is enabled if mode == 'simulation' and settings.get('run.collision_logging', False) and name == 'drive': from ..other.gazebo_collision_logger import GazeboCollisionLogger - other_components['collision_logger'] = mission_executor.make_component( + other_components['gazebo_collision_logger'] = mission_executor.make_component( {'type': 'GazeboCollisionLogger', 'module': 'gazebo_collision_logger'}, - 'collision_logger', + 'gazebo_collision_logger', 'GEMstack.onboard.other', {'vehicle_interface': vehicle_interface} ) @@ -153,6 +153,10 @@ def caution_callback(k,variant): mission_executor.log_vehicle_interface(log_vehicle_interface) #determine whether to log components log_components = log_settings.get('components',[]) + #add gazebo_collision_logger to components if collision logging is enabled + if settings.get('run.collision_logging', False): + if 'gazebo_collision_logger' not in log_components: + log_components.append('gazebo_collision_logger') mission_executor.log_components(log_components) #determine whether to log state log_state_attributes = log_settings.get('state',[]) diff --git a/GEMstack/onboard/other/gazebo_collision_logger.py b/GEMstack/onboard/other/gazebo_collision_logger.py index cb8ff8f3f..207c1f421 100644 --- a/GEMstack/onboard/other/gazebo_collision_logger.py +++ b/GEMstack/onboard/other/gazebo_collision_logger.py @@ -1,39 +1,24 @@ -from ...utils import settings from ..component import Component import rospy from gazebo_msgs.msg import ContactsState -from datetime import datetime -import os +from collections import deque +import time class GazeboCollisionLogger(Component): - """Component that logs collision data from Gazebo simulation.""" + """Logs collision data from Gazebo simulation.""" def __init__(self, vehicle_interface): super().__init__() self.vehicle_interface = vehicle_interface - self.log_file = None - self.initialized = False + self.collision_messages = deque(maxlen=100) + self.last_collision_time = None + self.last_collision_pair = None + def initialize(self): - """Initialize the collision logger component.""" - if not self.initialized: - # Get log folder from settings - log_settings = settings.get('run.log', {}) - topfolder = log_settings.get('folder', 'logs') - prefix = log_settings.get('prefix', '') - suffix = log_settings.get('suffix', datetime.now().strftime('%Y-%m-%d_%H-%M-%S')) - logfolder = os.path.join(topfolder, prefix + suffix) - - # Create collision log file - self.log_file = os.path.join(logfolder, 'collision_log.txt') - os.makedirs(os.path.dirname(self.log_file), exist_ok=True) - - # Subscribe to contact sensor topic - rospy.Subscriber('/contact_sensor', ContactsState, self.contact_callback) - - self.initialized = True - rospy.loginfo("Collision logger initialized. Logging to: %s", self.log_file) - rospy.loginfo("Waiting for collision events...") + """Initialize the collision logger.""" + self.contact_sub = rospy.Subscriber('/contact_sensor', ContactsState, self.contact_callback) + rospy.loginfo("Gazebo collision logger initialized") def simplify_collision_name(self, name): """Simplify collision names to be more readable.""" @@ -49,60 +34,43 @@ def simplify_collision_name(self, name): def contact_callback(self, msg): """Callback for processing collision messages.""" - if not msg.states: # If no contacts + if not msg.states: return - timestamp = datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3] - - # Create a dictionary to store unique collisions - unique_collisions = {} + current_time = time.time() # Process all collision states for state in msg.states: - collision_key = tuple(sorted([state.collision1_name, state.collision2_name])) - if collision_key not in unique_collisions or state.depths[0] > unique_collisions[collision_key].depths[0]: - unique_collisions[collision_key] = state - - # Log collision information - rospy.loginfo("\n=== Collision Detected at %s ===", timestamp) - - with open(self.log_file, 'a') as f: - f.write(f"\n=== Collision Detected at {timestamp} ===\n") + collision1 = self.simplify_collision_name(state.collision1_name) + collision2 = self.simplify_collision_name(state.collision2_name) - for i, (collision_key, state) in enumerate(unique_collisions.items(), 1): - collision1 = self.simplify_collision_name(state.collision1_name) - collision2 = self.simplify_collision_name(state.collision2_name) - - # Reorder collisions to put vehicle parts first - if "vehicle" in collision2: - collision1, collision2 = collision2, collision1 - - # Log collision details - rospy.loginfo("\nContact %d:", i) - rospy.loginfo("Collision between: %s and %s", collision1, collision2) + # Reorder collisions to put vehicle parts first + if "vehicle" in collision2: + collision1, collision2 = collision2, collision1 + + collision_pair = tuple(sorted([collision1, collision2])) + + # Only log if it's a new collision or enough time has passed + if (self.last_collision_time is None or + current_time - self.last_collision_time > 0.25 or # Different time + collision_pair != self.last_collision_pair): # Different collision pair pos = state.contact_positions[0] - pos_str = f"Contact Position: x={pos.x:.3f}, y={pos.y:.3f}, z={pos.z:.3f}" - rospy.loginfo(pos_str) - normal = state.contact_normals[0] - normal_str = f"Contact Normal: x={normal.x:.3f}, y={normal.y:.3f}, z={normal.z:.3f}" - rospy.loginfo(normal_str) - depth_str = f"Contact Depth: {state.depths[0]:.3f}" - rospy.loginfo(depth_str) + message = ( + f"Collision between: {collision1} and {collision2}\n" + f"Contact Position: x={pos.x:.3f}, y={pos.y:.3f}, z={pos.z:.3f}\n" + f"Contact Normal: x={normal.x:.3f}, y={normal.y:.3f}, z={normal.z:.3f}\n" + f"Contact Depth: {state.depths[0]:.3f}\n" + f"{'-' * 50}" + ) - # Write to file - f.write(f"\nContact {i}:\n") - f.write(f"Collision between: {collision1} and {collision2}\n") - f.write(f"{pos_str}\n") - f.write(f"{normal_str}\n") - f.write(f"{depth_str}\n") - f.write("-" * 50 + "\n") - - f.write("\n") - rospy.loginfo("-" * 50) + self.collision_messages.append(message) + self.last_collision_time = current_time + self.last_collision_pair = collision_pair def update(self): - """Update method required by Component base class.""" - pass # All work is done in the ROS callback \ No newline at end of file + """Output collision messages to the console/log.""" + while self.collision_messages: + print(self.collision_messages.popleft()) \ No newline at end of file diff --git a/docs/Gazebo Simulation Documentation.md b/docs/Gazebo Simulation Documentation.md index ba36bc9d2..23ac33e03 100644 --- a/docs/Gazebo Simulation Documentation.md +++ b/docs/Gazebo Simulation Documentation.md @@ -149,4 +149,18 @@ Follow the instructions in the [POLARIS GEM Simulator](https://github.com/harish The naming conventions for entities are important as they determine how models are detected and classified. Refer to the [Entity Detection Documentation](gazebo_entity_detection.md) for details on model naming and the detection process. +## Collision Logging in Gazebo + +To enable collision logging in your Gazebo simulation, add the following to your launch file's `gazebo` variant: + +```yaml +run: + collision_logging: True # Enable collision logging +``` + +When enabled, the collision logger will: +- Monitor collisions between the vehicle and other objects in the simulation +- Log collision details including colliding objects, contact position, normal, and depth +- Output logs to `GazeboCollisionLogger.stdout.log` in your log directory + ---