Merge pull request #218 from krishauser/gazebo_collision_detection

Gazebo collision detection

Author: krishauser

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -69,7 +69,10 @@ components:
       outputs: trajectory
   - trajectory_tracking:
       inputs: [vehicle, trajectory]
-      outputs: 
+      outputs:
   - signaling:
       inputs: [intent]
-      outputs:
+      outputs:
+  - gazebo_collision_logger:
+      inputs: []
+      outputs: 

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['gazebo_collision_logger'] = mission_executor.make_component(
+                {'type': 'GazeboCollisionLogger', 'module': 'gazebo_collision_logger'},
+                'gazebo_collision_logger',
+                'GEMstack.onboard.other',
+                {'vehicle_interface': vehicle_interface}
+            )
+        
         mission_executor.add_pipeline(name,perception_components,planning_components,other_components)
 
     #configure logging
@@ -143,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

@@ -0,0 +1,76 @@
+from ..component import Component
+import rospy
+from gazebo_msgs.msg import ContactsState
+from collections import deque
+import time
+
+class GazeboCollisionLogger(Component):
+    """Logs collision data from Gazebo simulation."""
+    
+    def __init__(self, vehicle_interface):
+        super().__init__()
+        self.vehicle_interface = vehicle_interface
+        self.collision_messages = deque(maxlen=100)
+        self.last_collision_time = None
+        self.last_collision_pair = None
+
+        
+    def initialize(self):
+        """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."""
+        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:
+            return
+            
+        current_time = time.time()
+        
+        # Process all collision states
+        for state in msg.states:
+            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
+            
+            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]
+                normal = state.contact_normals[0]
+                
+                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}"
+                )
+                
+                self.collision_messages.append(message)
+                self.last_collision_time = current_time
+                self.last_collision_pair = collision_pair
+    
+    def update(self):
+        """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
+
 ---

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

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: