updated collision logger to use gemstacks logger

Author: pravshot

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -73,6 +73,6 @@ components:
   - signaling:
       inputs: [intent]
       outputs:
-  - collision_logger:
+  - gazebo_collision_logger:
       inputs: []
-      outputs:
+      outputs: 

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',[])

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 
+        """Output collision messages to the console/log."""
+        while self.collision_messages:
+            print(self.collision_messages.popleft()) 

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
+
 ---