From 1c386902f60267d132fef654538f3703719b1337 Mon Sep 17 00:00:00 2001 From: Sanjay Pokkali Date: Sun, 11 May 2025 14:54:15 -0500 Subject: [PATCH 1/2] added obstacle detection pt1 --- .../onboard/perception/obstacle_detection.py | 56 +++++++++++++++++++ .../visualization/mpl_visualization.py | 13 +++++ GEMstack/state/__init__.py | 4 +- 3 files changed, 71 insertions(+), 2 deletions(-) create mode 100644 GEMstack/onboard/perception/obstacle_detection.py diff --git a/GEMstack/onboard/perception/obstacle_detection.py b/GEMstack/onboard/perception/obstacle_detection.py new file mode 100644 index 000000000..5654441f6 --- /dev/null +++ b/GEMstack/onboard/perception/obstacle_detection.py @@ -0,0 +1,56 @@ + +import threading +import copy +from typing import Dict +from ..component import Component + +from ..interface.gem import GEMInterface +from ...state.obstacle import ObstacleState + +class OmniscientObstacleDetector(Component): + """Obtains agent detections from a simulator""" + def __init__(self,vehicle_interface : GEMInterface): + self.vehicle_interface = vehicle_interface + self.obstacles = {} + self.lock = threading.Lock() + + def rate(self): + return 15.0 + + def state_inputs(self): + return [] + + def state_outputs(self): + return ['obstacles'] + + def initialize(self): + self.vehicle_interface.subscribe_sensor('obstacle_detector',self.obstacle_callback, ObstacleState) + + def obstacle_callback(self, name : str, obstacle : ObstacleState): + with self.lock: + self.obstacles[name] = obstacle + + def update(self) -> Dict[str,ObstacleState]: + with self.lock: + return copy.deepcopy(self.obstacles) + + +class GazeboObstacleDetector(OmniscientObstacleDetector): + """Obtains agent detections from the Gazebo simulator using model_states topic""" + def __init__(self, vehicle_interface : GEMInterface, tracked_obstacle_prefixes=None): + super().__init__(vehicle_interface) + + # If specific model prefixes are provided, configure the interface to track them + if tracked_obstacle_prefixes is not None: + # Check if our interface has the tracked_model_prefixes attribute (is a GazeboInterface) + if hasattr(vehicle_interface, 'tracked_obstacle_prefixes'): + vehicle_interface.tracked_obstacle_prefixes = tracked_obstacle_prefixes + print(f"Configured GazeboConeDetector to track obstacles with prefixes: {tracked_obstacle_prefixes}") + else: + print("Warning: vehicle_interface doesn't support tracked_obstacle_prefixes configuration") + + def initialize(self): + # Use the same agent_detector sensor as OmniscientAgentDetector + # The GazeboInterface implements this with model_states subscription + super().initialize() + print("GazeboConeDetector initialized and subscribed to model_states") \ No newline at end of file diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py index 3c35c50bc..790f51f16 100644 --- a/GEMstack/onboard/visualization/mpl_visualization.py +++ b/GEMstack/onboard/visualization/mpl_visualization.py @@ -97,6 +97,7 @@ def update(self, state): # Print debugging info about the vehicle's position and frame if self.num_updates % 10 == 0: print(f"Vehicle position: ({state.vehicle.pose.x:.2f}, {state.vehicle.pose.y:.2f}), frame: {state.vehicle.pose.frame}") + print("Obstacle state:", state.obstacles) # Pedestrian metrics and position debugging ped_positions = [] @@ -246,6 +247,18 @@ def update(self, state): except Exception as e: print(f"Error in pedestrian plot: {str(e)}") + try: + # ---- plot static obstacles in orange ---- + for obs in state.obstacles.values(): + x_obs, y_obs = obs.pose.x, obs.pose.y + self.axs[0].plot(x_obs, y_obs, 'o', + markersize=8, + markerfacecolor='orange', + markeredgecolor='darkorange', + label='_nolegend_') + except Exception as e: + print(f"Error plotting obstacles: {str(e)}") + # Update canvas try: self.fig.canvas.draw_idle() diff --git a/GEMstack/state/__init__.py b/GEMstack/state/__init__.py index 8ddc0c5b0..43bc3e19a 100644 --- a/GEMstack/state/__init__.py +++ b/GEMstack/state/__init__.py @@ -8,7 +8,7 @@ 'VehicleState', 'Roadgraph', 'Roadmap', - 'Obstacle', + 'Obstacle', 'ObstacleMaterialEnum','ObstacleStateEnum','ObstacleState', 'Sign', 'AgentState','AgentEnum','AgentActivityEnum', 'SceneState', @@ -23,7 +23,7 @@ from .trajectory import Path,Trajectory from .vehicle import VehicleState,VehicleGearEnum from .roadgraph import Roadgraph, RoadgraphLane, RoadgraphCurve, RoadgraphRegion, RoadgraphCurveEnum, RoadgraphLaneEnum, RoadgraphRegionEnum, RoadgraphSurfaceEnum, RoadgraphConnectionEnum -from .obstacle import Obstacle, ObstacleMaterialEnum +from .obstacle import Obstacle, ObstacleMaterialEnum, ObstacleStateEnum, ObstacleState from .sign import Sign, SignEnum, SignalLightEnum, SignState from .roadmap import Roadmap from .agent import AgentState, AgentEnum, AgentActivityEnum From 3d5468946e9089c3f4dee4ed0ef64c16af761a19 Mon Sep 17 00:00:00 2001 From: shraddhaamohan Date: Sun, 11 May 2025 14:55:44 -0500 Subject: [PATCH 2/2] added obstacle detection pt2 --- GEMstack/onboard/interface/gem_gazebo.py | 159 ++++++++++++++--------- GEMstack/state/obstacle.py | 24 +++- launch/fixed_route.yaml | 4 + 3 files changed, 124 insertions(+), 63 deletions(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 24f5d1378..5ac892f6c 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -1,3 +1,4 @@ +from GEMstack.state.obstacle import ObstacleMaterialEnum, ObstacleState, ObstacleStateEnum from .gem import * from ...utils import settings import math @@ -53,7 +54,13 @@ class GNSSReading: 'car': 'car', 'vehicle': 'car', 'truck': 'medium_truck', - 'large_truck': 'large_truck' + 'large_truck': 'large_truck', + 'cone': 'traffic_cone' +} + +# Map model prefixes to obstacle types +MODEL_PREFIX_TO_OBSTACLE_TYPE = { + 'cone': 'traffic_cone' } class GEMGazeboInterface(GEMInterface): @@ -93,11 +100,15 @@ def __init__(self): self.model_states_sub = None self.tracked_model_prefixes = settings.get('simulator.agent_tracker.model_prefixes', ['pedestrian', 'bicycle', 'car']) + self.tracked_obstacle_prefixes = settings.get('simulator.obstacle_tracker.model_prefixes', + ['cone']) self.agent_detector_callback = None + self.obstacle_detector_callback = None self.last_agent_positions = {} self.last_agent_velocities = {} self.last_model_states_time = 0.0 self.agent_detection_rate = settings.get('simulator.agent_tracker.rate', 10.0) # Hz + self.obstacle_detection_rate = settings.get('simulator.obstacle_tracker.rate', 50.0) # Hz # Frame transformation variables self.start_pose_abs = None # Initial vehicle pose in GLOBAL frame @@ -141,7 +152,7 @@ def model_states_callback(self, msg: ModelStates): current_time = self.time() # Check if we should process this update (rate limiting) - if current_time - self.last_model_states_time < 1.0/self.agent_detection_rate: + if ((current_time - self.last_model_states_time < 1.0/self.agent_detection_rate) and (current_time - self.last_model_states_time < 1.0/self.obstacle_detection_rate)): return # Calculate time delta since last update @@ -149,7 +160,7 @@ def model_states_callback(self, msg: ModelStates): self.last_model_states_time = current_time # Skip if no callback is registered - if self.agent_detector_callback is None: + if self.agent_detector_callback is None and self.obstacle_detector_callback is None: return # Find vehicle in model states @@ -203,16 +214,24 @@ def model_states_callback(self, msg: ModelStates): agent_type = None for prefix in self.tracked_model_prefixes: if model_name.lower().startswith(prefix.lower()): - # Find the appropriate agent type from the prefix for key, value in MODEL_PREFIX_TO_AGENT_TYPE.items(): if prefix.lower().startswith(key.lower()): agent_type = value break break + + obstacle_type = None + for prefix in self.tracked_obstacle_prefixes: + if model_name.lower().startswith(prefix.lower()): + for key, value in MODEL_PREFIX_TO_OBSTACLE_TYPE.items(): + if prefix.lower().startswith(key.lower()): + obstacle_type = value + break + break - if agent_type is None: + if agent_type is None and obstacle_type is None: continue # Not an agent we're tracking - + # Get position and orientation from model states position = msg.pose[i].position orientation = msg.pose[i].orientation @@ -224,7 +243,6 @@ def model_states_callback(self, msg: ModelStates): # Convert orientation quaternion to euler angles quaternion = (orientation.x, orientation.y, orientation.z, orientation.w) roll, pitch, yaw = euler_from_quaternion(quaternion) - # Create agent pose in ABSOLUTE_CARTESIAN frame (Gazebo's native frame) agent_global_pose = ObjectPose( frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, @@ -269,61 +287,72 @@ def model_states_callback(self, msg: ModelStates): # If transformation not initialized yet, just use the model_states pose agent_pose = agent_global_pose - # Calculate velocity manually if twist data is zero or missing - velocity = (linear_vel.x, linear_vel.y, linear_vel.z) - velocity_is_zero = abs(linear_vel.x) < 1e-6 and abs(linear_vel.y) < 1e-6 and abs(linear_vel.z) < 1e-6 - - if velocity_is_zero and model_name in self.last_agent_positions and dt > 0: - # Calculate velocity from position difference - prev_pos = self.last_agent_positions[model_name] - dx = position.x - prev_pos[0] - dy = position.y - prev_pos[1] - dz = position.z - prev_pos[2] + if agent_type is not None: + # Calculate velocity manually if twist data is zero or missing + velocity = (linear_vel.x, linear_vel.y, linear_vel.z) + velocity_is_zero = abs(linear_vel.x) < 1e-6 and abs(linear_vel.y) < 1e-6 and abs(linear_vel.z) < 1e-6 - # Calculate velocity (position change / time) - calculated_vel = (dx/dt, dy/dt, dz/dt) + if velocity_is_zero and model_name in self.last_agent_positions and dt > 0: + # Calculate velocity from position difference + prev_pos = self.last_agent_positions[model_name] + dx = position.x - prev_pos[0] + dy = position.y - prev_pos[1] + dz = position.z - prev_pos[2] + + # Calculate velocity (position change / time) + calculated_vel = (dx/dt, dy/dt, dz/dt) + + # Apply some smoothing with the previous velocity if available + if model_name in self.last_agent_velocities: + prev_vel = self.last_agent_velocities[model_name] + # Apply exponential smoothing (0.7 current + 0.3 previous) + velocity = ( + 0.7 * calculated_vel[0] + 0.3 * prev_vel[0], + 0.7 * calculated_vel[1] + 0.3 * prev_vel[1], + 0.7 * calculated_vel[2] + 0.3 * prev_vel[2] + ) + else: + velocity = calculated_vel - # Apply some smoothing with the previous velocity if available - if model_name in self.last_agent_velocities: - prev_vel = self.last_agent_velocities[model_name] - # Apply exponential smoothing (0.7 current + 0.3 previous) - velocity = ( - 0.7 * calculated_vel[0] + 0.3 * prev_vel[0], - 0.7 * calculated_vel[1] + 0.3 * prev_vel[1], - 0.7 * calculated_vel[2] + 0.3 * prev_vel[2] - ) + # Determine activity state based on velocity magnitude + velocity_magnitude = np.linalg.norm(velocity) + if velocity_magnitude < 0.1: + activity = AgentActivityEnum.STOPPED + elif velocity_magnitude > 5.0: # Arbitrary threshold for "fast" + activity = AgentActivityEnum.FAST else: - velocity = calculated_vel + activity = AgentActivityEnum.MOVING + + # Get agent dimensions + dimensions = AGENT_DIMENSIONS.get(agent_type, (1.0, 1.0, 1.0)) # Default if unknown - # Determine activity state based on velocity magnitude - velocity_magnitude = np.linalg.norm(velocity) - if velocity_magnitude < 0.1: - activity = AgentActivityEnum.STOPPED - elif velocity_magnitude > 5.0: # Arbitrary threshold for "fast" - activity = AgentActivityEnum.FAST - else: - activity = AgentActivityEnum.MOVING + # Create agent state + agent_state = AgentState( + pose=agent_pose, # Using START frame pose + dimensions=dimensions, + outline=None, + type=getattr(AgentEnum, agent_type.upper()), + activity=activity, + velocity=velocity, + yaw_rate=angular_vel.z + ) - # Get agent dimensions - dimensions = AGENT_DIMENSIONS.get(agent_type, (1.0, 1.0, 1.0)) # Default if unknown - - # Create agent state - agent_state = AgentState( - pose=agent_pose, # Using START frame pose - dimensions=dimensions, - outline=None, - type=getattr(AgentEnum, agent_type.upper()), - activity=activity, - velocity=velocity, - yaw_rate=angular_vel.z - ) - - # Store current position for next velocity calculation (using raw positions) - self.last_agent_positions[model_name] = (position.x, position.y, position.z) - self.last_agent_velocities[model_name] = velocity - - # Call the callback with the agent state - self.agent_detector_callback(model_name, agent_state) + # Store current position for next velocity calculation (using raw positions) + self.last_agent_positions[model_name] = (position.x, position.y, position.z) + self.last_agent_velocities[model_name] = velocity + # Call the callback with the agent state + self.agent_detector_callback(model_name, agent_state) + + if obstacle_type is not None: + # Create obstacle state + obstacle_state = ObstacleState( + dimensions=(0,0,0), + outline=None, + pose=agent_pose, + type=getattr(ObstacleMaterialEnum, obstacle_type.upper()), + activity=ObstacleStateEnum.STANDING, + ) + self.obstacle_detector_callback(model_name, obstacle_state) def subscribe_sensor(self, name, callback, type=None): if name == 'gnss': @@ -424,6 +453,11 @@ def callback_with_cv2(msg: Image): raise ValueError("GEMGazeboInterface only supports AgentState for agent_detector") self.agent_detector_callback = callback + elif name == 'obstacle_detector': + if type is not None and type is not ObstacleState: + raise ValueError("GEMGazeboInterface only supports ObstacleState for obstacle_detector") + self.obstacle_detector_callback = callback + def hardware_faults(self) -> List[str]: # In simulation, we don't have real hardware faults return self.faults @@ -459,10 +493,12 @@ def send_command(self, command : GEMVehicleCommand): # Calculate acceleration from pedal positions acceleration = pedal_positions_to_acceleration(accelerator_pedal_position, brake_pedal_position, v, 0, 1) - + print("acceleration before", acceleration) # Apply reasonable limits to acceleration max_accel = settings.get('vehicle.limits.max_acceleration', 1.0) - max_decel = settings.get('vehicle.limits.max_deceleration', -2.0) + max_decel = -1 * settings.get('vehicle.limits.max_deceleration', 2.0) # cuz ackermann expects neg but pure pursiut wants positive decel val + print("max_accel", max_accel) + print("max_decel", max_decel) acceleration = np.clip(acceleration, max_decel, max_accel) # Convert wheel angle to steering angle (front wheel angle) @@ -477,7 +513,7 @@ def send_command(self, command : GEMVehicleCommand): # Don't use infinite speed, instead calculate a reasonable target speed current_speed = v target_speed = current_speed - + print("acceleration ", acceleration) if acceleration > 0: # Accelerating - set target speed to current speed plus some increment # This is more realistic than infinite speed @@ -485,6 +521,7 @@ def send_command(self, command : GEMVehicleCommand): target_speed = min(current_speed + acceleration * 0.5, max_speed) elif acceleration < 0: # Braking - set target speed to zero if deceleration is significant + print("braking ", acceleration) if brake_pedal_position > 0.1: target_speed = 0.0 @@ -499,4 +536,4 @@ def send_command(self, command : GEMVehicleCommand): print(f"[ACKERMANN] Speed: {msg.speed:.2f}, Accel: {msg.acceleration:.2f}, Steer: {msg.steering_angle:.2f}") self.ackermann_pub.publish(msg) - self.last_command = command + self.last_command = command \ No newline at end of file diff --git a/GEMstack/state/obstacle.py b/GEMstack/state/obstacle.py index 8fddd5cc0..df8965062 100644 --- a/GEMstack/state/obstacle.py +++ b/GEMstack/state/obstacle.py @@ -1,6 +1,7 @@ -from dataclasses import dataclass +from __future__ import annotations +from dataclasses import dataclass, replace from ..utils.serialization import register -from .physical_object import PhysicalObject +from .physical_object import ObjectFrameEnum,PhysicalObject #,convert_vector from enum import Enum class ObstacleMaterialEnum(Enum): @@ -16,6 +17,14 @@ class ObstacleMaterialEnum(Enum): SMALL_ANIMAL = 9 ROADKILL = 10 +class ObstacleStateEnum(Enum): + STOPPED = 0 # standing pedestrians, parked cars, etc. No need to predict motion. + MOVING = 1 # standard motion. Predictions will be used here + FAST = 2 # indicates faster than usual motion + UNDETERMINED = 3 # unknown activity + STANDING = 4 # standing cone + LEFT = 5 # flipped cone facing left + RIGHT = 6 # flipped cone facing right @dataclass @register @@ -23,3 +32,14 @@ class Obstacle(PhysicalObject): material : ObstacleMaterialEnum collidable : bool + +@dataclass +@register +class ObstacleState(PhysicalObject): + type: ObstacleMaterialEnum + activity: ObstacleStateEnum + + def to_frame(self, frame: ObjectFrameEnum, current_pose=None, start_pose_abs=None) -> ObstacleState: + newpose = self.pose.to_frame(frame, current_pose, start_pose_abs) + # newvelocity = convert_vector(self.velocity, self.pose.frame, frame, current_pose, start_pose_abs) + return replace(self, pose=newpose) #, velocity=newvelocity) \ No newline at end of file diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 59b027fcc..58d83b43b 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -86,6 +86,10 @@ variants: type: agent_detection.GazeboAgentDetector args: tracked_model_prefixes: ['pedestrian', 'car', 'bicycle'] + obstacle_detection: + type: obstacle_detection.GazeboObstacleDetector + args: + tracked_obstacle_prefixes: ['cone'] visualization: !include "mpl_visualization.yaml" log_ros: log: