diff --git a/GEMstack/knowledge/defaults/computation_graph.yaml b/GEMstack/knowledge/defaults/computation_graph.yaml index aa79c7cbc..505b85684 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: \ No newline at end of file + outputs: + - gazebo_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..849450e15 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['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',[]) diff --git a/GEMstack/onboard/other/gazebo_collision_logger.py b/GEMstack/onboard/other/gazebo_collision_logger.py new file mode 100644 index 000000000..207c1f421 --- /dev/null +++ b/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()) \ 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 + --- 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: