From 2f719955da396e09b20f3720e099c6f6eae6dbe0 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Wed, 30 Apr 2025 00:17:49 -0500 Subject: [PATCH 01/13] WIP agent detection and tracking from gazebo model states --- .gitignore | 2 - .../knowledge/calibration/gem_e4_gazebo.yaml | 69 +------- GEMstack/onboard/interface/gem_gazebo.py | 163 +++++++++++++++++- .../perception/README_agent_detection.md | 85 +++++++++ .../onboard/perception/agent_detection.py | 21 +++ .../visualization/mpl_visualization.py | 93 ++++++++-- GEMstack/utils/mpl_visualization.py | 66 ++++++- launch/fixed_route.yaml | 17 +- 8 files changed, 427 insertions(+), 89 deletions(-) create mode 100644 GEMstack/onboard/perception/README_agent_detection.md diff --git a/.gitignore b/.gitignore index 61a2df38b..3da224293 100644 --- a/.gitignore +++ b/.gitignore @@ -171,8 +171,6 @@ cython_debug/ .vscode/ setup/zed_sdk.run cuda/ -homework/yolov8n.pt -homework/yolo11n.pt GEMstack/knowledge/detection/yolov8n.pt GEMstack/knowledge/detection/yolo11n.pt yolov8n.pt diff --git a/GEMstack/knowledge/calibration/gem_e4_gazebo.yaml b/GEMstack/knowledge/calibration/gem_e4_gazebo.yaml index d6dfc3185..26d04c0a7 100644 --- a/GEMstack/knowledge/calibration/gem_e4_gazebo.yaml +++ b/GEMstack/knowledge/calibration/gem_e4_gazebo.yaml @@ -1,64 +1,7 @@ -date: '2025-02-25 22:31:02.057447' -calibration_type: absolute_vehicle_calibration -gnss_location: -- 1.1 -- 0 -- 1.62 -gnss_yaw: 0.0 -rear_axle_height: 0.33 -reference: rear_axle_center -vehicle: gem_e4 +calibration_date: "2024-03-05" # Date of calibration YYYY-MM-DD +reference: rear_axle_center # rear axle center +rear_axle_height: 0.33 # height of rear axle center above flat ground +gnss_location: [1.10,0,1.62] # meters, taken from https://github.com/hangcui1201/POLARIS_GEM_e2_Real/blob/main/vehicle_drivers/gem_gnss_control/scripts/gem_gnss_tracker_stanley_rtk.py. Note conflict with pure pursuit location? +gnss_yaw: 0.0 # radians top_lidar: !include "gem_e4_ouster.yaml" -front_camera: !include "gem_e4_oak.yaml" -gnss_reference_point: [40.114001, -88.228837] -T_frontcamera_vehicle: -- - 0.04979815921110274 - - -0.02889308605338095 - - 0.998341277156963 - - 0.6874899022540225 -- - -0.9986898492450907 - - 0.010343220052680617 - - 0.05011489006575621 - - -0.006750728511462573 -- - -0.011774038000135968 - - -0.9995289850272167 - - -0.02834016107658961 - - -0.07694233627246522 -- - 0.0 - - 0.0 - - 0.0 - - 1.0 -T_toplidar_frontcamera: -- - 0.029096143490519977 - - -0.9995463314295703 - - -0.007781115579835407 - - -0.045768971801312515 -- - -0.005777780139254174 - - 0.007616104089319731 - - -0.999954305063568 - - -0.3868171492765085 -- - 0.9995599190096819 - - 0.02913977151915098 - - -0.005553559684576381 - - -0.6975440651652165 -- - 0.0 - - 0.0 - - 0.0 - - 1.0 -T_toplidar_vehicle: -- - 0.9995177984237671 - - -0.020904183387756348 - - 0.022959932684898376 - - 0.0 -- - 0.020975051447749138 - - 0.9997758865356445 - - -0.0028501423075795174 - - 0.0 -- - -0.0228952094912529 - - 0.003330353880301118 - - 0.9997323155403137 - - 0.33000001311302185 -- - 0.0 - - 0.0 - - 0.0 - - 1.0 \ No newline at end of file +front_camera: !include "gem_e4_oak.yaml" \ No newline at end of file diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 575334136..dd4b98e9f 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -11,12 +11,14 @@ # Changed from AckermannDriveStamped from ackermann_msgs.msg import AckermannDrive from rosgraph_msgs.msg import Clock +from gazebo_msgs.msg import ModelStates from tf.transformations import euler_from_quaternion from ...state import ObjectPose,ObjectFrameEnum from ...knowledge.vehicle.geometry import steer2front from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration +from ...state import AgentState, AgentEnum, AgentActivityEnum # OpenCV and cv2 bridge import cv2 @@ -32,6 +34,27 @@ class GNSSReading: status: str +# Agent dimensions similar to what's in gem_simulator.py +AGENT_DIMENSIONS = { + 'pedestrian' : (0.5,0.5,1.6), + 'bicyclist' : (1.8,0.5,1.6), + 'car' : (4.0,2.5,1.4), + 'medium_truck': (6.0,2.5,3.0), + 'large_truck': (10.0,2.5,3.5) +} + +# Map model prefixes to agent types +MODEL_PREFIX_TO_AGENT_TYPE = { + 'pedestrian': 'pedestrian', + 'person': 'pedestrian', + 'bicycle': 'bicyclist', + 'bike': 'bicyclist', + 'car': 'car', + 'vehicle': 'car', + 'truck': 'medium_truck', + 'large_truck': 'large_truck' +} + class GEMGazeboInterface(GEMInterface): """Interface for connecting to the GEM e2 vehicle in Gazebo simulation.""" @@ -52,10 +75,6 @@ def __init__(self): self.last_reading.wiper_level = 0 self.last_reading.headlights_on = False - - - - # IMU data subscriber self.imu_sub = None self.imu_data = None @@ -69,6 +88,16 @@ def __init__(self): self.top_lidar_sub = None self.front_radar_sub = None + # Agent detection + self.model_states_sub = None + self.tracked_model_prefixes = settings.get('simulator.agent_tracker.model_prefixes', + ['pedestrian', 'person', 'bicycle', 'bike', 'car', 'vehicle', 'truck']) + self.agent_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.faults = [] # Gazebo vehicle control @@ -87,6 +116,9 @@ def __init__(self): self.sim_time = rospy.Time(0) self.clock_sub = rospy.Subscriber('/clock', Clock, self.clock_callback) + # Subscribe to model states for agent detection + self.model_states_sub = rospy.Subscriber('/gazebo/model_states', ModelStates, self.model_states_callback) + def start(self): print("Starting GEM Gazebo Interface") @@ -106,6 +138,120 @@ def gnss_vel_callback(self, msg): def get_reading(self) -> GEMVehicleReading: return self.last_reading + 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: + return + + # Calculate time delta since last update + dt = current_time - self.last_model_states_time + self.last_model_states_time = current_time + + # Skip if no callback is registered + if self.agent_detector_callback is None: + return + + # Process all models + for i, model_name in enumerate(msg.name): + # Check if this model should be tracked as an agent + 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 + + if agent_type is None: + continue # Not an agent we're tracking + + # Get position and orientation + position = msg.pose[i].position + orientation = msg.pose[i].orientation + + # Get velocity from twist + linear_vel = msg.twist[i].linear + angular_vel = msg.twist[i].angular + + # Convert orientation quaternion to euler angles + quaternion = (orientation.x, orientation.y, orientation.z, orientation.w) + roll, pitch, yaw = euler_from_quaternion(quaternion) + + # Create object pose + pose = ObjectPose( + frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, + t=current_time, + x=position.x, + y=position.y, + z=position.z, + roll=roll, + pitch=pitch, + yaw=yaw + ) + + # 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] + + # 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 + + # Debug output for manual velocity calculation + if np.linalg.norm(velocity) > 0.01: + print(f"Calculated velocity for {model_name}: {velocity}") + + # 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 + + # 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=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 + 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) def subscribe_sensor(self, name, callback, type=None): if name == 'gnss': @@ -210,11 +356,20 @@ def callback_with_cv2(msg: Image): cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="passthrough") callback(cv_image) self.front_depth_sub = rospy.Subscriber(topic, Image, callback_with_cv2) + + elif name == 'agent_detector': + if type is not None and type is not AgentState: + raise ValueError("GEMGazeboInterface only supports AgentState for agent_detector") + self.agent_detector_callback = callback def hardware_faults(self) -> List[str]: # In simulation, we don't have real hardware faults return self.faults + def sensors(self): + # Add agent_detector to the list of available sensors + return super().sensors() + ['agent_detector'] + def send_command(self, command : GEMVehicleCommand): # Throttle rate at which we send commands t = self.time() diff --git a/GEMstack/onboard/perception/README_agent_detection.md b/GEMstack/onboard/perception/README_agent_detection.md new file mode 100644 index 000000000..195db2d63 --- /dev/null +++ b/GEMstack/onboard/perception/README_agent_detection.md @@ -0,0 +1,85 @@ +# Agent Detection in GEMstack + +This module contains implementations for detecting agents (pedestrians, vehicles, etc.) in the environment. The available detectors are: + +## OmniscientAgentDetector + +The `OmniscientAgentDetector` works with the basic simulator to obtain agent states that are being simulated. It receives updates from the simulator and maintains a thread-safe dictionary of all detected agents. + +Example usage: + +```python +from onboard.perception.agent_detection import OmniscientAgentDetector +from onboard.interface.gem_simulator import GEMDoubleIntegratorSimulationInterface + +# Create the simulator interface +simulator = GEMDoubleIntegratorSimulationInterface() + +# Create the agent detector with the simulator interface +agent_detector = OmniscientAgentDetector(simulator) + +# Initialize and start +agent_detector.initialize() +simulator.start() + +# Later, when you need agent data: +agent_states = agent_detector.update() +``` + +## GazeboAgentDetector + +The `GazeboAgentDetector` works specifically with the Gazebo simulator, subscribing to the model states topic and converting Gazebo models into `AgentState` objects. It can be configured to track specific model prefixes. + +Example usage: + +```python +from onboard.perception.agent_detection import GazeboAgentDetector +from onboard.interface.gem_gazebo import GEMGazeboInterface + +# Create the Gazebo interface +gazebo_interface = GEMGazeboInterface() + +# Define the model prefixes you want to track as agents +tracked_prefixes = ['pedestrian', 'person', 'car', 'vehicle'] + +# Create the agent detector with the Gazebo interface and the tracked prefixes +agent_detector = GazeboAgentDetector(gazebo_interface, tracked_prefixes) + +# Initialize and start +agent_detector.initialize() +gazebo_interface.start() + +# Later, when you need agent data: +agent_states = agent_detector.update() +``` + +## Configuration + +The Gazebo agent detector can be configured through settings in the configuration file: + +```yaml +simulator: + agent_tracker: + model_prefixes: + - pedestrian + - person + - bicycle + - bike + - car + - vehicle + - truck + rate: 10.0 # Hz - how frequently to process model updates +``` + +The model prefixes are strings that match the beginning of Gazebo model names. For example, a model named "pedestrian1" would match the "pedestrian" prefix. + +## Agent State Format + +Both detectors provide `AgentState` objects with the following properties: + +- `pose`: The position and orientation of the agent +- `dimensions`: The physical dimensions (length, width, height) of the agent +- `type`: The type of agent (car, pedestrian, bicyclist, etc.) +- `activity`: The activity state of the agent (stopped, moving, fast) +- `velocity`: The velocity vector in the agent's local frame +- `yaw_rate`: The rate of rotation around the vertical axis \ No newline at end of file diff --git a/GEMstack/onboard/perception/agent_detection.py b/GEMstack/onboard/perception/agent_detection.py index 5d600f792..c80442771 100644 --- a/GEMstack/onboard/perception/agent_detection.py +++ b/GEMstack/onboard/perception/agent_detection.py @@ -31,3 +31,24 @@ def agent_callback(self, name : str, agent : AgentState): def update(self) -> Dict[str,AgentState]: with self.lock: return copy.deepcopy(self.agents) + + +class GazeboAgentDetector(OmniscientAgentDetector): + """Obtains agent detections from the Gazebo simulator using model_states topic""" + def __init__(self, vehicle_interface : GEMInterface, tracked_model_prefixes=None): + super().__init__(vehicle_interface) + + # If specific model prefixes are provided, configure the interface to track them + if tracked_model_prefixes is not None: + # Check if our interface has the tracked_model_prefixes attribute (is a GazeboInterface) + if hasattr(vehicle_interface, 'tracked_model_prefixes'): + vehicle_interface.tracked_model_prefixes = tracked_model_prefixes + print(f"Configured GazeboAgentDetector to track models with prefixes: {tracked_model_prefixes}") + else: + print("Warning: vehicle_interface doesn't support tracked_model_prefixes configuration") + + def initialize(self): + # Use the same agent_detector sensor as OmniscientAgentDetector + # The GazeboInterface implements this with model_states subscription + super().initialize() + print("GazeboAgentDetector initialized and subscribed to model_states") diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py index 08392e9ed..861917e5c 100644 --- a/GEMstack/onboard/visualization/mpl_visualization.py +++ b/GEMstack/onboard/visualization/mpl_visualization.py @@ -93,21 +93,94 @@ def update(self, state): self.debug("vehicle", "velocity", state.vehicle.v) self.debug("vehicle", "front_wheel_angle", state.vehicle.front_wheel_angle) - # Pedestrian metrics + # Pedestrian metrics and position debugging + ped_positions = [] for agent_id, agent in state.agents.items(): if agent.type == AgentEnum.PEDESTRIAN: - # self.debug(f"ped_{agent_id}", "x", agent.pose.x) - # self.debug(f"ped_{agent_id}", "y", agent.pose.y) - self.debug(f"ped_{agent_id}", "velocity", np.linalg.norm(agent.velocity)) # Magnitude of resultant velocity + # Position logging + ped_x, ped_y = agent.pose.x, agent.pose.y + ped_positions.append((ped_x, ped_y)) + # Debug output every 10 updates + if self.num_updates % 10 == 0: + print(f"Pedestrian {agent_id} position: ({ped_x:.2f}, {ped_y:.2f}), frame: {agent.pose.frame}") + # Calculate distance from vehicle + dist = np.sqrt((ped_x - state.vehicle.pose.x)**2 + (ped_y - state.vehicle.pose.y)**2) + print(f"Distance to vehicle: {dist:.2f} meters") + + # Track positions for plotting + self.debug(f"ped_{agent_id}", "x", ped_x) + self.debug(f"ped_{agent_id}", "y", ped_y) + self.debug(f"ped_{agent_id}", "velocity", np.linalg.norm(agent.velocity)) self.debug(f"ped_{agent_id}", "yaw_rate", agent.yaw_rate) time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(state.t)) - #frame=ObjectFrameEnum.CURRENT - #state = state.to_frame(frame) - xrange = [state.vehicle.pose.x - 10, state.vehicle.pose.x + 10] - yrange = [state.vehicle.pose.y - 10, state.vehicle.pose.y + 10] - #plot main visualization - mpl_visualization.plot(state,title="Scene %d at %s"%(self.num_updates,time_str),xrange=xrange,yrange=yrange,show=False,ax=self.axs[0]) + + # Print coordinate frame information for debugging + if self.num_updates % 10 == 0: + print(f"Vehicle: pos=({state.vehicle.pose.x:.2f}, {state.vehicle.pose.y:.2f}), frame={state.vehicle.pose.frame}") + if state.start_vehicle_pose: + print(f"Start pose: pos=({state.start_vehicle_pose.x:.2f}, {state.start_vehicle_pose.y:.2f}), frame={state.start_vehicle_pose.frame}") + + if len(state.agents) > 0: + print(f"Number of agents: {len(state.agents)}") + + # Determine a good plot range that includes both vehicle and pedestrians + if len(ped_positions) > 0: + # Start with vehicle position + min_x, max_x = state.vehicle.pose.x, state.vehicle.pose.x + min_y, max_y = state.vehicle.pose.y, state.vehicle.pose.y + + # Expand to include pedestrians + for x, y in ped_positions: + min_x = min(min_x, x) + max_x = max(max_x, x) + min_y = min(min_y, y) + max_y = max(max_y, y) + + # Add margin - based on the content size + size_x = max(50, max_x - min_x) + size_y = max(50, max_y - min_y) + margin_x = max(20, size_x * 0.2) # at least 20m or 20% of content + margin_y = max(20, size_y * 0.2) + + xrange = [min_x - margin_x, max_x + margin_x] + yrange = [min_y - margin_y, max_y + margin_y] + else: + # Default range around vehicle if no pedestrians + xrange = [state.vehicle.pose.x - 10, state.vehicle.pose.x + 10] + yrange = [state.vehicle.pose.y - 10, state.vehicle.pose.y + 10] + + # Print xrange and yrange for debugging + if self.num_updates % 10 == 0: + print(f"Plot range: X {xrange}, Y {yrange}") + + # Try converting state to START frame for visualization + try: + if state.start_vehicle_pose is not None: + state_start = state.to_frame(ObjectFrameEnum.START) + print(f"Successfully converted state to START frame") + + # Log the conversion for comparison + if len(state.agents) > 0 and self.num_updates % 10 == 0: + for agent_id, agent in state.agents.items(): + # Get the agent in both frames + agent_start = state_start.agents.get(agent_id) + if agent_start: + print(f"Agent {agent_id} in ABSOLUTE: ({agent.pose.x:.2f}, {agent.pose.y:.2f})") + print(f"Agent {agent_id} in START: ({agent_start.pose.x:.2f}, {agent_start.pose.y:.2f})") + + # Try using the START frame for visualization + mpl_visualization.plot(state_start, title=f"START Frame: Scene {self.num_updates}", + xrange=xrange, yrange=yrange, show=False, ax=self.axs[0]) + else: + # Use original state if no start pose + mpl_visualization.plot(state, title=f"Scene {self.num_updates} at {time_str}", + xrange=xrange, yrange=yrange, show=False, ax=self.axs[0]) + except Exception as e: + print(f"Error converting to START frame: {str(e)}") + # Fallback to the original state + mpl_visualization.plot(state, title=f"Scene {self.num_updates} at {time_str}", + xrange=xrange, yrange=yrange, show=False, ax=self.axs[0]) # Vehicle plot (axs[1]) self.axs[1].clear() diff --git a/GEMstack/utils/mpl_visualization.py b/GEMstack/utils/mpl_visualization.py index 09d078a02..b027fc59c 100644 --- a/GEMstack/utils/mpl_visualization.py +++ b/GEMstack/utils/mpl_visualization.py @@ -67,6 +67,12 @@ def plot_object(obj : PhysicalObject, axis_len=None, outline=True, bbox=True, ax #plot bounding box R = obj.pose.rotation2d() t = [obj.pose.x,obj.pose.y] + + # Debug print every 50 calls (to avoid flooding console) + import random + if random.random() < 0.02: # ~2% chance to print + print(f"Plotting object at position ({obj.pose.x:.2f}, {obj.pose.y:.2f}) with frame {obj.pose.frame}") + if bbox or (outline and obj.outline is None): bounds = obj.bounds() (xmin,xmax),(ymin,ymax),(zmin,zmax) = bounds @@ -79,6 +85,7 @@ def plot_object(obj : PhysicalObject, axis_len=None, outline=True, bbox=True, ax ax.plot(xs,ys,'r-') else: ax.plot(xs,ys,'b-') + #plot outline if outline and obj.outline: outline = [R.dot(p)+t for p in obj.outline] @@ -86,6 +93,24 @@ def plot_object(obj : PhysicalObject, axis_len=None, outline=True, bbox=True, ax xs = [c[0] for c in outline] ys = [c[1] for c in outline] ax.plot(xs,ys,'r-') + + # Add a marker at the center to make small agents more visible + if isinstance(obj, AgentState): + # Make different agent types visually distinct + if obj.type == AgentEnum.PEDESTRIAN: + marker = 'o' + markersize = 10 + color = 'r' + elif obj.type == AgentEnum.BICYCLIST: + marker = 's' + markersize = 10 + color = 'g' + else: # vehicles + marker = 'D' + markersize = 8 + color = 'b' + + ax.plot(obj.pose.x, obj.pose.y, marker=marker, markersize=markersize, color=color) def plot_vehicle(vehicle : VehicleState, axis_len=0.1, ax=None): """Plots the vehicle in the given axes. The coordinates @@ -228,13 +253,42 @@ def plot_scene(scene : SceneState, xrange=None, yrange=None, ax=None, title = No else: ax.set_ylim(-yrange*0.5,yrange*0.5) #plot roadgraph - plot_roadgraph(scene.roadgraph,scene.route,ax=ax) + try: + if scene.roadgraph is not None: + plot_roadgraph(scene.roadgraph,scene.route,ax=ax) + except Exception as e: + print(f"Error plotting roadgraph: {str(e)}") + #plot vehicle and objects - plot_vehicle(scene.vehicle,ax=ax) - for k,a in scene.agents.items(): - plot_object(a,ax=ax) - for k,o in scene.obstacles.items(): - plot_object(o,ax=ax) + try: + if scene.vehicle is not None: + plot_vehicle(scene.vehicle,ax=ax) + except Exception as e: + print(f"Error plotting vehicle: {str(e)}") + + try: + if scene.agents: + # Print agent count for debugging + print(f"Plotting {len(scene.agents)} agents") + for agent_id, agent in scene.agents.items(): + try: + print(f"Agent {agent_id}: {agent.type}, pos=({agent.pose.x:.2f}, {agent.pose.y:.2f}), frame={agent.pose.frame}") + plot_object(agent,ax=ax) + except Exception as e: + print(f"Error plotting agent {agent_id}: {str(e)}") + except Exception as e: + print(f"Error plotting agents: {str(e)}") + + try: + if scene.obstacles: + for k,o in scene.obstacles.items(): + try: + plot_object(o,ax=ax) + except Exception as e: + print(f"Error plotting obstacle {k}: {str(e)}") + except Exception as e: + print(f"Error plotting obstacles: {str(e)}") + if title is None: if show: ax.set_title("Scene at t = %.2f" % scene.t) diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 40e6b9a73..6ef499f71 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -38,7 +38,7 @@ log: # If True, then record all readings / commands of the vehicle interface. Default False vehicle_interface : True # Specify which components to record to behavior.json. Default records nothing - components : ['state_estimation','trajectory_tracking'] + components : ['state_estimation','trajectory_tracking', 'agent_detection'] # Specify which components of state to record to state.json. Default records nothing #state: ['all'] # Specify the rate in Hz at which to record state to state.json. Default records at the pipeline's rate @@ -72,8 +72,8 @@ variants: perception: state_estimation : OmniscientStateEstimator agent_detection : OmniscientAgentDetector - visualization: !include "klampt_visualization.yaml" - #visualization: !include "mpl_visualization.yaml" + # visualization: !include "klampt_visualization.yaml" + visualization: [!include "mpl_visualization.yaml", !include "klampt_visualization.yaml"] gazebo: run: mode: simulation @@ -82,7 +82,16 @@ variants: drive: perception: state_estimation: GNSSStateEstimator # Matches your Gazebo GNSS implementation - # visualization: !include "mpl_visualization.yaml" + agent_detection: + type: agent_detection.GazeboAgentDetector + args: + tracked_model_prefixes: ['pedestrian', 'person', 'car', 'vehicle', 'bicycle'] + # Configure Gazebo agent tracking + simulator: + agent_tracker: + model_prefixes: ['pedestrian', 'person', 'car', 'vehicle', 'bicycle', 'bike', 'truck'] + rate: 10.0 # Hz + visualization: !include "mpl_visualization.yaml" log_ros: log: ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file From 34eb95155ee8740cf951a6de7dd556edf5e391c9 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sun, 4 May 2025 17:35:39 -0500 Subject: [PATCH 02/13] WIP fix before transform --- .../visualization/mpl_visualization.py | 162 ++++++++++++------ GEMstack/utils/mpl_visualization.py | 85 ++++++--- 2 files changed, 166 insertions(+), 81 deletions(-) diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py index 861917e5c..3c35c50bc 100644 --- a/GEMstack/onboard/visualization/mpl_visualization.py +++ b/GEMstack/onboard/visualization/mpl_visualization.py @@ -6,6 +6,7 @@ import time from collections import deque from ...state.agent import AgentEnum +from ...state import ObjectFrameEnum, ObjectPose, AllState import numpy as np class MPLVisualization(Component): @@ -93,25 +94,53 @@ def update(self, state): self.debug("vehicle", "velocity", state.vehicle.v) self.debug("vehicle", "front_wheel_angle", state.vehicle.front_wheel_angle) + # 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}") + # Pedestrian metrics and position debugging ped_positions = [] for agent_id, agent in state.agents.items(): - if agent.type == AgentEnum.PEDESTRIAN: - # Position logging - ped_x, ped_y = agent.pose.x, agent.pose.y - ped_positions.append((ped_x, ped_y)) - # Debug output every 10 updates - if self.num_updates % 10 == 0: - print(f"Pedestrian {agent_id} position: ({ped_x:.2f}, {ped_y:.2f}), frame: {agent.pose.frame}") - # Calculate distance from vehicle - dist = np.sqrt((ped_x - state.vehicle.pose.x)**2 + (ped_y - state.vehicle.pose.y)**2) - print(f"Distance to vehicle: {dist:.2f} meters") + try: + # Check agent type safely + is_pedestrian = False + try: + is_pedestrian = agent.type == AgentEnum.PEDESTRIAN + except: + # If there's an issue comparing, try string comparison + try: + is_pedestrian = str(agent.type) == str(AgentEnum.PEDESTRIAN) + except: + # If that fails, use substring match + try: + is_pedestrian = "PEDESTRIAN" in str(agent.type) + except: + # Last resort: assume it's a pedestrian if not explicitly marked as something else + is_pedestrian = True - # Track positions for plotting - self.debug(f"ped_{agent_id}", "x", ped_x) - self.debug(f"ped_{agent_id}", "y", ped_y) - self.debug(f"ped_{agent_id}", "velocity", np.linalg.norm(agent.velocity)) - self.debug(f"ped_{agent_id}", "yaw_rate", agent.yaw_rate) + if is_pedestrian: + # Position logging + ped_x, ped_y = agent.pose.x, agent.pose.y + ped_positions.append((ped_x, ped_y)) + # Debug output every 10 updates + if self.num_updates % 10 == 0: + print(f"Pedestrian {agent_id} position: ({ped_x:.2f}, {ped_y:.2f}), frame: {agent.pose.frame}") + # Calculate distance from vehicle + dist = np.sqrt((ped_x - state.vehicle.pose.x)**2 + (ped_y - state.vehicle.pose.y)**2) + print(f"Distance to vehicle: {dist:.2f} meters") + + # Track positions for plotting + self.debug(f"ped_{agent_id}", "x", ped_x) + self.debug(f"ped_{agent_id}", "y", ped_y) + # Calculate velocity magnitude safely + try: + vel_mag = np.linalg.norm(agent.velocity) + except: + vel_mag = 0.0 + self.debug(f"ped_{agent_id}", "velocity", vel_mag) + self.debug(f"ped_{agent_id}", "yaw_rate", agent.yaw_rate) + except Exception as e: + print(f"Error processing agent {agent_id}: {str(e)}") time_str = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(state.t)) @@ -154,56 +183,75 @@ def update(self, state): if self.num_updates % 10 == 0: print(f"Plot range: X {xrange}, Y {yrange}") - # Try converting state to START frame for visualization + # Plot using the current state directly - avoid conversions that might fail try: - if state.start_vehicle_pose is not None: - state_start = state.to_frame(ObjectFrameEnum.START) - print(f"Successfully converted state to START frame") + # First attempt: Just use current state as-is + mpl_visualization.plot(state, title=f"Scene {self.num_updates} at {time_str}", + xrange=xrange, yrange=yrange, show=False, ax=self.axs[0]) + except Exception as e: + print(f"Error in basic plot: {str(e)}") + # If that fails, try a minimal plot with just essential elements + try: + # Fallback to a minimal plot without using state conversion + self.axs[0].clear() + self.axs[0].set_aspect('equal') + self.axs[0].set_xlabel('x (m)') + self.axs[0].set_ylabel('y (m)') + self.axs[0].set_xlim(xrange[0], xrange[1]) + self.axs[0].set_ylim(yrange[0], yrange[1]) - # Log the conversion for comparison - if len(state.agents) > 0 and self.num_updates % 10 == 0: - for agent_id, agent in state.agents.items(): - # Get the agent in both frames - agent_start = state_start.agents.get(agent_id) - if agent_start: - print(f"Agent {agent_id} in ABSOLUTE: ({agent.pose.x:.2f}, {agent.pose.y:.2f})") - print(f"Agent {agent_id} in START: ({agent_start.pose.x:.2f}, {agent_start.pose.y:.2f})") + # Just draw dots for vehicle and agents + self.axs[0].plot(state.vehicle.pose.x, state.vehicle.pose.y, 'bo', markersize=10, label='Vehicle') - # Try using the START frame for visualization - mpl_visualization.plot(state_start, title=f"START Frame: Scene {self.num_updates}", - xrange=xrange, yrange=yrange, show=False, ax=self.axs[0]) - else: - # Use original state if no start pose - mpl_visualization.plot(state, title=f"Scene {self.num_updates} at {time_str}", - xrange=xrange, yrange=yrange, show=False, ax=self.axs[0]) - except Exception as e: - print(f"Error converting to START frame: {str(e)}") - # Fallback to the original state - mpl_visualization.plot(state, title=f"Scene {self.num_updates} at {time_str}", - xrange=xrange, yrange=yrange, show=False, ax=self.axs[0]) + # Plot pedestrians as red dots + for x, y in ped_positions: + self.axs[0].plot(x, y, 'ro', markersize=8) + + self.axs[0].set_title(f"Basic Scene {self.num_updates} at {time_str}") + self.axs[0].legend() + except Exception as e2: + print(f"Even basic plot failed: {str(e2)}") # Vehicle plot (axs[1]) - self.axs[1].clear() - for k,v in self.vehicle_plot_values.items(): - t = [x[0] for x in v] - y = [x[1] for x in v] - self.axs[1].plot(t,y,label=k) - self.axs[1].set_title('Vehicle Metrics') - self.axs[1].set_xlabel('Time (s)') - self.axs[1].legend() + try: + self.axs[1].clear() + has_data = False + for k,v in self.vehicle_plot_values.items(): + if len(v) > 0: # Only plot if we have data + t = [x[0] for x in v] + y = [x[1] for x in v] + self.axs[1].plot(t,y,label=k) + has_data = True + if has_data: + self.axs[1].legend() + self.axs[1].set_title('Vehicle Metrics') + self.axs[1].set_xlabel('Time (s)') + except Exception as e: + print(f"Error in vehicle plot: {str(e)}") # Pedestrian plot (axs[2]) - self.axs[2].clear() - for k,v in self.pedestrian_plot_values.items(): - t = [x[0] for x in v] - y = [x[1] for x in v] - self.axs[2].plot(t,y,label=k) - self.axs[2].set_title('Pedestrian Metrics') - self.axs[2].set_xlabel('Time (s)') - self.axs[2].legend() + try: + self.axs[2].clear() + has_data = False + for k,v in self.pedestrian_plot_values.items(): + if len(v) > 0: # Only plot if we have data + t = [x[0] for x in v] + y = [x[1] for x in v] + self.axs[2].plot(t,y,label=k) + has_data = True + if has_data: + self.axs[2].legend() + self.axs[2].set_title('Pedestrian Metrics') + self.axs[2].set_xlabel('Time (s)') + except Exception as e: + print(f"Error in pedestrian plot: {str(e)}") - self.fig.canvas.draw_idle() - self.fig.canvas.flush_events() + # Update canvas + try: + self.fig.canvas.draw_idle() + self.fig.canvas.flush_events() + except Exception as e: + print(f"Error updating canvas: {str(e)}") if self.save_as is not None and self.writer is not None: try: diff --git a/GEMstack/utils/mpl_visualization.py b/GEMstack/utils/mpl_visualization.py index b027fc59c..abf8ab7d9 100644 --- a/GEMstack/utils/mpl_visualization.py +++ b/GEMstack/utils/mpl_visualization.py @@ -3,6 +3,7 @@ import numpy as np from . import settings from ..state import ObjectFrameEnum,ObjectPose,PhysicalObject,VehicleState,VehicleGearEnum,Path,Obstacle,AgentState,Roadgraph,RoadgraphLane,RoadgraphLaneEnum,RoadgraphCurve,RoadgraphCurveEnum,RoadgraphRegion,RoadgraphRegionEnum,RoadgraphSurfaceEnum,Trajectory,Route,SceneState,AllState +from ..state.agent import AgentEnum CURVE_TO_STYLE = { RoadgraphCurveEnum.LANE_BOUNDARY : {'color':'k','linewidth':1,'linestyle':'-'}, @@ -68,7 +69,7 @@ def plot_object(obj : PhysicalObject, axis_len=None, outline=True, bbox=True, ax R = obj.pose.rotation2d() t = [obj.pose.x,obj.pose.y] - # Debug print every 50 calls (to avoid flooding console) + # Debug print occasionally import random if random.random() < 0.02: # ~2% chance to print print(f"Plotting object at position ({obj.pose.x:.2f}, {obj.pose.y:.2f}) with frame {obj.pose.frame}") @@ -95,22 +96,45 @@ def plot_object(obj : PhysicalObject, axis_len=None, outline=True, bbox=True, ax ax.plot(xs,ys,'r-') # Add a marker at the center to make small agents more visible - if isinstance(obj, AgentState): - # Make different agent types visually distinct - if obj.type == AgentEnum.PEDESTRIAN: - marker = 'o' - markersize = 10 - color = 'r' - elif obj.type == AgentEnum.BICYCLIST: - marker = 's' - markersize = 10 - color = 'g' - else: # vehicles - marker = 'D' - markersize = 8 - color = 'b' - - ax.plot(obj.pose.x, obj.pose.y, marker=marker, markersize=markersize, color=color) + try: + if isinstance(obj, AgentState): + # Make different agent types visually distinct + try: + # Try to identify pedestrians + is_pedestrian = False + try: + is_pedestrian = obj.type == AgentEnum.PEDESTRIAN + except: + # If direct comparison fails, try string comparison + try: + is_pedestrian = str(obj.type) == str(AgentEnum.PEDESTRIAN) + except: + # If string comparison fails, try substring match + try: + is_pedestrian = "PEDESTRIAN" in str(obj.type) + except: + # Last resort + is_pedestrian = False + + # Set marker style based on agent type + if is_pedestrian: + marker = 'o' + markersize = 10 + color = 'r' + else: + # Default for other agent types + marker = 's' + markersize = 8 + color = 'b' + + ax.plot(obj.pose.x, obj.pose.y, marker=marker, markersize=markersize, color=color) + + except Exception as e: + # Fallback to basic marker if type identification fails + ax.plot(obj.pose.x, obj.pose.y, 'mo', markersize=8) + print(f"Using basic marker for agent due to error: {str(e)}") + except Exception as e: + print(f"Error adding agent marker: {str(e)}") def plot_vehicle(vehicle : VehicleState, axis_len=0.1, ax=None): """Plots the vehicle in the given axes. The coordinates @@ -252,36 +276,47 @@ def plot_scene(scene : SceneState, xrange=None, yrange=None, ax=None, title = No ax.set_ylim(yrange[0],yrange[1]) else: ax.set_ylim(-yrange*0.5,yrange*0.5) - #plot roadgraph + + # Plot roadgraph if available try: if scene.roadgraph is not None: plot_roadgraph(scene.roadgraph,scene.route,ax=ax) except Exception as e: print(f"Error plotting roadgraph: {str(e)}") - #plot vehicle and objects + # Plot vehicle if available try: if scene.vehicle is not None: plot_vehicle(scene.vehicle,ax=ax) except Exception as e: print(f"Error plotting vehicle: {str(e)}") + # Fallback to basic marker for vehicle + try: + ax.plot(scene.vehicle.pose.x, scene.vehicle.pose.y, 'bo', markersize=10) + except: + pass + # Plot agents with careful error handling try: if scene.agents: - # Print agent count for debugging print(f"Plotting {len(scene.agents)} agents") for agent_id, agent in scene.agents.items(): try: - print(f"Agent {agent_id}: {agent.type}, pos=({agent.pose.x:.2f}, {agent.pose.y:.2f}), frame={agent.pose.frame}") plot_object(agent,ax=ax) - except Exception as e: - print(f"Error plotting agent {agent_id}: {str(e)}") + except Exception as agent_e: + print(f"Error plotting agent {agent_id}: {str(agent_e)}") + # Fallback to a simple marker for this agent + try: + ax.plot(agent.pose.x, agent.pose.y, 'ro', markersize=8) + except: + pass except Exception as e: print(f"Error plotting agents: {str(e)}") + # Plot obstacles if available try: if scene.obstacles: - for k,o in scene.obstacles.items(): + for k, o in scene.obstacles.items(): try: plot_object(o,ax=ax) except Exception as e: @@ -289,11 +324,13 @@ def plot_scene(scene : SceneState, xrange=None, yrange=None, ax=None, title = No except Exception as e: print(f"Error plotting obstacles: {str(e)}") + # Set title if title is None: if show: ax.set_title("Scene at t = %.2f" % scene.t) else: ax.set_title(title) + if show: plt.show(block=False) From 810e027da3811d9e5083101895204443374f190e Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sun, 4 May 2025 18:57:53 -0500 Subject: [PATCH 03/13] fix agent state with transform --- GEMstack/onboard/interface/gem_gazebo.py | 122 +++++++++++++++++++++-- 1 file changed, 111 insertions(+), 11 deletions(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index dd4b98e9f..e21f07c89 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -2,6 +2,7 @@ from ...utils import settings import math import time +import random # ROS Headers import rospy @@ -97,7 +98,17 @@ def __init__(self): self.last_agent_velocities = {} self.last_model_states_time = 0.0 self.agent_detection_rate = settings.get('simulator.agent_tracker.rate', 10.0) # Hz - + + # Frame transformation variables + self.start_pose_abs = None # Initial vehicle pose in GLOBAL frame + self.vehicle_model_pose = None # Current vehicle pose from model_states + self.vehicle_gps_pose = None # Current vehicle pose from GPS + self.t_start = None # Start time + + # Stable transformation variables + self.transform_initialized = False + self.initial_vehicle_model_pose = None # Vehicle pose in model_states at start + self.faults = [] # Gazebo vehicle control @@ -152,9 +163,54 @@ def model_states_callback(self, msg: ModelStates): # Skip if no callback is registered if self.agent_detector_callback is None: return + + # Find vehicle in model states + vehicle_idx = -1 + for i, name in enumerate(msg.name): + if name.lower() in ['gem_e4', 'gem_e2', 'polaris', 'golf_cart', 'vehicle']: + vehicle_idx = i + break + + # If vehicle not found, cannot proceed + if vehicle_idx < 0: + return - # Process all models + # Get vehicle position and orientation from model states + vehicle_pos = msg.pose[vehicle_idx].position + vehicle_ori = msg.pose[vehicle_idx].orientation + quaternion = (vehicle_ori.x, vehicle_ori.y, vehicle_ori.z, vehicle_ori.w) + _, _, vehicle_yaw = euler_from_quaternion(quaternion) + + # Create vehicle model pose in ABSOLUTE_CARTESIAN frame (Gazebo's native frame) + self.vehicle_model_pose = ObjectPose( + frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, # Gazebo uses its own coordinate system + t=current_time, + x=vehicle_pos.x, + y=vehicle_pos.y, + z=vehicle_pos.z, + yaw=vehicle_yaw + ) + + # Initialize stable transformation when we have both GPS and model data + if not self.transform_initialized and self.vehicle_gps_pose is not None: + # Initialize start pose and transformation data + self.start_pose_abs = self.vehicle_gps_pose + self.initial_vehicle_model_pose = self.vehicle_model_pose + self.t_start = current_time + self.transform_initialized = True + + print("STABLE TRANSFORMATION INITIALIZED:") + print(f" GPS position: ({self.start_pose_abs.x:.4f}, {self.start_pose_abs.y:.4f}, {self.start_pose_abs.z:.4f})") + print(f" Model position: ({self.initial_vehicle_model_pose.x:.4f}, {self.initial_vehicle_model_pose.y:.4f}, {self.initial_vehicle_model_pose.z:.4f})") + print(f" GPS orientation: {self.start_pose_abs.yaw:.4f} radians") + print(f" Model orientation: {self.initial_vehicle_model_pose.yaw:.4f} radians") + + # Process all models except the vehicle itself for i, model_name in enumerate(msg.name): + # Skip the vehicle model itself + if i == vehicle_idx: + continue + # Check if this model should be tracked as an agent agent_type = None for prefix in self.tracked_model_prefixes: @@ -169,7 +225,7 @@ def model_states_callback(self, msg: ModelStates): if agent_type is None: continue # Not an agent we're tracking - # Get position and orientation + # Get position and orientation from model states position = msg.pose[i].position orientation = msg.pose[i].orientation @@ -181,18 +237,59 @@ def model_states_callback(self, msg: ModelStates): quaternion = (orientation.x, orientation.y, orientation.z, orientation.w) roll, pitch, yaw = euler_from_quaternion(quaternion) - # Create object pose - pose = ObjectPose( - frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, + # Debug print occasionally + if random.random() < 0.01: # 1% chance to print + print(f"Agent {model_name} raw model_state position: ({position.x:.2f}, {position.y:.2f}, {position.z:.2f})") + + # Create agent pose in ABSOLUTE_CARTESIAN frame (Gazebo's native frame) + agent_global_pose = ObjectPose( + frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, # Gazebo's coordinate system t=current_time, x=position.x, - y=position.y, - z=position.z, + y=position.y, + z=position.z, roll=roll, pitch=pitch, yaw=yaw ) + # Transform agent pose to START frame using stable transformation + agent_pose = None + if self.transform_initialized: + # Calculate agent position relative to the *initial* vehicle position (from when START frame was established) + # This provides a stable transformation that doesn't change as the vehicle moves + rel_x = position.x - self.initial_vehicle_model_pose.x + rel_y = position.y - self.initial_vehicle_model_pose.y + rel_z = position.z - self.initial_vehicle_model_pose.z + + # Rotate by the *initial* vehicle orientation + cos_yaw = math.cos(-self.initial_vehicle_model_pose.yaw) + sin_yaw = math.sin(-self.initial_vehicle_model_pose.yaw) + rot_x = rel_x * cos_yaw - rel_y * sin_yaw + rot_y = rel_x * sin_yaw + rel_y * cos_yaw + + # Adjust yaw relative to *initial* vehicle orientation + rel_yaw = yaw - self.initial_vehicle_model_pose.yaw + + # Create the pose in START frame using the stable transformation + agent_pose = ObjectPose( + frame=ObjectFrameEnum.START, + t=current_time - self.t_start if self.t_start is not None else 0, + x=rot_x, + y=rot_y, + z=rel_z, + roll=roll, + pitch=pitch, + yaw=rel_yaw + ) + + # Debug print occasionally + if random.random() < 0.01: # 1% chance to print + print(f"Agent {model_name} stable relative position: ({rot_x:.2f}, {rot_y:.2f}, {rel_z:.2f})") + else: + # 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 @@ -237,7 +334,7 @@ def model_states_callback(self, msg: ModelStates): # Create agent state agent_state = AgentState( - pose=pose, + pose=agent_pose, # Using START frame pose dimensions=dimensions, outline=None, type=getattr(AgentEnum, agent_type.upper()), @@ -246,7 +343,7 @@ def model_states_callback(self, msg: ModelStates): yaw_rate=angular_vel.z ) - # Store current position for next velocity calculation + # 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 @@ -294,6 +391,9 @@ def gnss_callback_wrapper(gps_msg: NavSatFix): pitch=pitch, yaw=navigation_yaw ) + + # Save the vehicle's GPS pose for coordinate transformation + self.vehicle_gps_pose = pose # Create GNSS reading with fused data reading = GNSSReading( @@ -313,7 +413,7 @@ def gnss_callback_wrapper(gps_msg: NavSatFix): callback(reading) self.gnss_sub = rospy.Subscriber(topic, NavSatFix, gnss_callback_wrapper) - + elif name == 'top_lidar': topic = self.ros_sensor_topics[name] if type is not None and (type is not PointCloud2 and type is not np.ndarray): From 153ce23b39f311e4307442d6a9f08877026f7655 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sun, 4 May 2025 20:46:38 -0500 Subject: [PATCH 04/13] update docs --- GEMstack/onboard/interface/gem_gazebo.py | 2 +- docs/Gazebo Simulation Documentation.md | 49 ++++++++++++++++++++++++ launch/fixed_route.yaml | 7 +--- 3 files changed, 51 insertions(+), 7 deletions(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index e21f07c89..1c4b7388e 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -167,7 +167,7 @@ def model_states_callback(self, msg: ModelStates): # Find vehicle in model states vehicle_idx = -1 for i, name in enumerate(msg.name): - if name.lower() in ['gem_e4', 'gem_e2', 'polaris', 'golf_cart', 'vehicle']: + if name.lower() in ['gem_e4', 'gem_e2']: vehicle_idx = i break diff --git a/docs/Gazebo Simulation Documentation.md b/docs/Gazebo Simulation Documentation.md index d2f51bb03..56143fa45 100644 --- a/docs/Gazebo Simulation Documentation.md +++ b/docs/Gazebo Simulation Documentation.md @@ -88,3 +88,52 @@ python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/fixed_route.yaml - e2_gazebo - e4_gazebo --- + +## Agent Detection in Gazebo + +The Gazebo simulation environment supports detection of various types of agents (pedestrians, vehicles, etc.) that can be spawned in the simulation world. You can configure GEMstack to detect and track these agents. + +### Configuring Agent Detection in Launch File + +To enable agent detection in your Gazebo simulation, add the following to your launch file's `gazebo` variant: + +```yaml +drive: + perception: + state_estimation: GNSSStateEstimator # Matches your Gazebo GNSS implementation + agent_detection: + type: agent_detection.GazeboAgentDetector + args: + tracked_model_prefixes: ['pedestrian', 'car', 'bicycle'] +``` + +The `tracked_model_prefixes` parameter specifies which types of models will be tracked as agents. The prefixes listed will match against the model names in Gazebo. For example, a model named `pedestrian1` will be tracked if `pedestrian` is in the list. + +### Spawning Agents in Gazebo + +You can spawn agents in Gazebo using a YAML configuration file. + +Follow the instructions in the [POLARIS GEM Simulator](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) repository to spawn agents in Gazebo. + +### Important Naming Considerations + +The `name` field in your agent configuration is crucial as it determines how models are identified in Gazebo's `model_states` topic. For agent detection to work: + +1. The model name prefix must match one of the prefixes in your `tracked_model_prefixes` configuration +2. For example, a model named `pedestrian1` will be detected as a pedestrian if `pedestrian` is in your tracked prefixes +3. The model type is derived from the prefix, so naming conventions are important + +When spawning models in Gazebo, ensure that their names have appropriate prefixes that match your GEMstack configuration. + +### How Agent Detection Works + +The `GazeboAgentDetector` component: +1. Subscribes to Gazebo's `model_states` topic to get positions of all models +2. Filters models based on the configured `tracked_model_prefixes` +3. Transforms model positions from Gazebo's coordinate frame to GEMstack's START frame +4. Creates `AgentState` objects for each detected agent +5. Makes these agents available to other GEMstack components for planning and visualization + +The models in your simulation will appear in your GEMstack visualization and be available for planning and decision-making components to interact with. + +--- diff --git a/launch/fixed_route.yaml b/launch/fixed_route.yaml index 6ef499f71..59b027fcc 100644 --- a/launch/fixed_route.yaml +++ b/launch/fixed_route.yaml @@ -85,12 +85,7 @@ variants: agent_detection: type: agent_detection.GazeboAgentDetector args: - tracked_model_prefixes: ['pedestrian', 'person', 'car', 'vehicle', 'bicycle'] - # Configure Gazebo agent tracking - simulator: - agent_tracker: - model_prefixes: ['pedestrian', 'person', 'car', 'vehicle', 'bicycle', 'bike', 'truck'] - rate: 10.0 # Hz + tracked_model_prefixes: ['pedestrian', 'car', 'bicycle'] visualization: !include "mpl_visualization.yaml" log_ros: log: From cf751692956ac9113be8e7be6bf705e88ffd28e0 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sun, 4 May 2025 22:46:28 -0500 Subject: [PATCH 05/13] cleanup --- GEMstack/onboard/interface/gem_gazebo.py | 21 +++------------------ GEMstack/utils/mpl_visualization.py | 5 ----- 2 files changed, 3 insertions(+), 23 deletions(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 1c4b7388e..41bf6ee4f 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -1,8 +1,6 @@ from .gem import * from ...utils import settings import math -import time -import random # ROS Headers import rospy @@ -92,7 +90,7 @@ def __init__(self): # Agent detection self.model_states_sub = None self.tracked_model_prefixes = settings.get('simulator.agent_tracker.model_prefixes', - ['pedestrian', 'person', 'bicycle', 'bike', 'car', 'vehicle', 'truck']) + ['pedestrian', 'bicycle', 'car']) self.agent_detector_callback = None self.last_agent_positions = {} self.last_agent_velocities = {} @@ -183,7 +181,7 @@ def model_states_callback(self, msg: ModelStates): # Create vehicle model pose in ABSOLUTE_CARTESIAN frame (Gazebo's native frame) self.vehicle_model_pose = ObjectPose( - frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, # Gazebo uses its own coordinate system + frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, t=current_time, x=vehicle_pos.x, y=vehicle_pos.y, @@ -237,13 +235,9 @@ def model_states_callback(self, msg: ModelStates): quaternion = (orientation.x, orientation.y, orientation.z, orientation.w) roll, pitch, yaw = euler_from_quaternion(quaternion) - # Debug print occasionally - if random.random() < 0.01: # 1% chance to print - print(f"Agent {model_name} raw model_state position: ({position.x:.2f}, {position.y:.2f}, {position.z:.2f})") - # Create agent pose in ABSOLUTE_CARTESIAN frame (Gazebo's native frame) agent_global_pose = ObjectPose( - frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, # Gazebo's coordinate system + frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, t=current_time, x=position.x, y=position.y, @@ -257,7 +251,6 @@ def model_states_callback(self, msg: ModelStates): agent_pose = None if self.transform_initialized: # Calculate agent position relative to the *initial* vehicle position (from when START frame was established) - # This provides a stable transformation that doesn't change as the vehicle moves rel_x = position.x - self.initial_vehicle_model_pose.x rel_y = position.y - self.initial_vehicle_model_pose.y rel_z = position.z - self.initial_vehicle_model_pose.z @@ -282,10 +275,6 @@ def model_states_callback(self, msg: ModelStates): pitch=pitch, yaw=rel_yaw ) - - # Debug print occasionally - if random.random() < 0.01: # 1% chance to print - print(f"Agent {model_name} stable relative position: ({rot_x:.2f}, {rot_y:.2f}, {rel_z:.2f})") else: # If transformation not initialized yet, just use the model_states pose agent_pose = agent_global_pose @@ -315,10 +304,6 @@ def model_states_callback(self, msg: ModelStates): ) else: velocity = calculated_vel - - # Debug output for manual velocity calculation - if np.linalg.norm(velocity) > 0.01: - print(f"Calculated velocity for {model_name}: {velocity}") # Determine activity state based on velocity magnitude velocity_magnitude = np.linalg.norm(velocity) diff --git a/GEMstack/utils/mpl_visualization.py b/GEMstack/utils/mpl_visualization.py index abf8ab7d9..ee8ed1565 100644 --- a/GEMstack/utils/mpl_visualization.py +++ b/GEMstack/utils/mpl_visualization.py @@ -69,11 +69,6 @@ def plot_object(obj : PhysicalObject, axis_len=None, outline=True, bbox=True, ax R = obj.pose.rotation2d() t = [obj.pose.x,obj.pose.y] - # Debug print occasionally - import random - if random.random() < 0.02: # ~2% chance to print - print(f"Plotting object at position ({obj.pose.x:.2f}, {obj.pose.y:.2f}) with frame {obj.pose.frame}") - if bbox or (outline and obj.outline is None): bounds = obj.bounds() (xmin,xmax),(ymin,ymax),(zmin,zmax) = bounds From 661056c8794b605aef8bd7e692a89728fdfa41e7 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Sun, 4 May 2025 23:01:54 -0500 Subject: [PATCH 06/13] add deps in setup scripts --- docs/Gazebo Simulation Documentation.md | 4 +--- setup/Dockerfile.cuda11.8 | 2 +- setup/Dockerfile.cuda12 | 2 +- setup/setup_this_machine.sh | 2 +- 4 files changed, 4 insertions(+), 6 deletions(-) diff --git a/docs/Gazebo Simulation Documentation.md b/docs/Gazebo Simulation Documentation.md index f8c5703d7..783b7e0f4 100644 --- a/docs/Gazebo Simulation Documentation.md +++ b/docs/Gazebo Simulation Documentation.md @@ -25,7 +25,7 @@ Follow the instructions in the linked repo to build and run the Docker container Install the required ROS packages: ```bash -sudo apt-get install -y ros-noetic-ackermann-msgs +sudo apt-get install -y ros-noetic-ackermann-msgs ros-noetic-gazebo-msgs ``` --- @@ -100,8 +100,6 @@ GEMstack/knowledge/defaults/ - e2_gazebo.yaml - e4_gazebo.yaml ---- - ## Agent Detection in Gazebo The Gazebo simulation environment supports detection of various types of agents (pedestrians, vehicles, etc.) that can be spawned in the simulation world. You can configure GEMstack to detect and track these agents. diff --git a/setup/Dockerfile.cuda11.8 b/setup/Dockerfile.cuda11.8 index 459e7dca0..13d0da6e4 100644 --- a/setup/Dockerfile.cuda11.8 +++ b/setup/Dockerfile.cuda11.8 @@ -68,7 +68,7 @@ COPY requirements.txt /tmp/requirements.txt RUN pip3 install -r /tmp/requirements.txt # Install other Dependencies -RUN apt-get install -y ros-noetic-septentrio-gnss-driver ros-noetic-ackermann-msgs +RUN apt-get install -y ros-noetic-septentrio-gnss-driver ros-noetic-ackermann-msgs ros-noetic-gazebo-msgs USER ${USER} diff --git a/setup/Dockerfile.cuda12 b/setup/Dockerfile.cuda12 index a7ad71e82..2a628e5ea 100644 --- a/setup/Dockerfile.cuda12 +++ b/setup/Dockerfile.cuda12 @@ -69,7 +69,7 @@ COPY requirements.txt /tmp/requirements.txt RUN pip3 install -r /tmp/requirements.txt # Install other Dependencies -RUN apt-get install -y ros-noetic-septentrio-gnss-driver ros-noetic-ackermann-msgs +RUN apt-get install -y ros-noetic-septentrio-gnss-driver ros-noetic-ackermann-msgs ros-noetic-gazebo-msgs USER ${USER} diff --git a/setup/setup_this_machine.sh b/setup/setup_this_machine.sh index 36690f1d4..573e652b1 100755 --- a/setup/setup_this_machine.sh +++ b/setup/setup_this_machine.sh @@ -85,4 +85,4 @@ pip3 install -r $temp_requirements rm $temp_requirements #install other dependencies -sudo apt-get install -y ros-noetic-septentrio-gnss-driver \ No newline at end of file +sudo apt-get install -y ros-noetic-septentrio-gnss-driver ros-noetic-ackermann-msgs ros-noetic-gazebo-msgs \ No newline at end of file From 1c386902f60267d132fef654538f3703719b1337 Mon Sep 17 00:00:00 2001 From: Sanjay Pokkali Date: Sun, 11 May 2025 14:54:15 -0500 Subject: [PATCH 07/13] 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 08/13] 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: From 9c7ee060f3b93a5cd9b9fd9b5b263f3bcd46ed49 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Mon, 12 May 2025 01:50:30 -0500 Subject: [PATCH 09/13] update debug lines --- GEMstack/onboard/interface/gem_gazebo.py | 28 ++++++++++++------- .../visualization/mpl_visualization.py | 2 +- 2 files changed, 19 insertions(+), 11 deletions(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 27fcc04a1..1bfb7ecf0 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -207,11 +207,12 @@ def model_states_callback(self, msg: ModelStates): self.t_start = current_time self.transform_initialized = True - print("STABLE TRANSFORMATION INITIALIZED:") - print(f" GPS position: ({self.start_pose_abs.x:.4f}, {self.start_pose_abs.y:.4f}, {self.start_pose_abs.z:.4f})") - print(f" Model position: ({self.initial_vehicle_model_pose.x:.4f}, {self.initial_vehicle_model_pose.y:.4f}, {self.initial_vehicle_model_pose.z:.4f})") - print(f" GPS orientation: {self.start_pose_abs.yaw:.4f} radians") - print(f" Model orientation: {self.initial_vehicle_model_pose.yaw:.4f} radians") + if self.debug: + print("STABLE TRANSFORMATION INITIALIZED:") + print(f" GPS position: ({self.start_pose_abs.x:.4f}, {self.start_pose_abs.y:.4f}, {self.start_pose_abs.z:.4f})") + print(f" Model position: ({self.initial_vehicle_model_pose.x:.4f}, {self.initial_vehicle_model_pose.y:.4f}, {self.initial_vehicle_model_pose.z:.4f})") + print(f" GPS orientation: {self.start_pose_abs.yaw:.4f} radians") + print(f" Model orientation: {self.initial_vehicle_model_pose.yaw:.4f} radians") # Process all models except the vehicle itself for i, model_name in enumerate(msg.name): @@ -541,12 +542,15 @@ 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) + if self.debug: + print("acceleration before", acceleration) + # Apply reasonable limits to acceleration max_accel = settings.get('vehicle.limits.max_acceleration', 1.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) + if self.debug: + 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) @@ -561,7 +565,9 @@ 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 self.debug: + print("acceleration ", acceleration) + if acceleration > 0: # Accelerating - set target speed to current speed plus some increment # This is more realistic than infinite speed @@ -569,7 +575,9 @@ 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 self.debug: + print("braking ", acceleration) + if brake_pedal_position > 0.1: target_speed = 0.0 diff --git a/GEMstack/onboard/visualization/mpl_visualization.py b/GEMstack/onboard/visualization/mpl_visualization.py index 790f51f16..a251c98a1 100644 --- a/GEMstack/onboard/visualization/mpl_visualization.py +++ b/GEMstack/onboard/visualization/mpl_visualization.py @@ -97,7 +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) + # print("Obstacle state:", state.obstacles) # Pedestrian metrics and position debugging ped_positions = [] From 13704294b816595d33c09c89c6aa6601580f0ff1 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Mon, 12 May 2025 02:06:36 -0500 Subject: [PATCH 10/13] refactor gem_gazebo --- GEMstack/onboard/interface/gem_gazebo.py | 243 ++++++++++++++--------- 1 file changed, 148 insertions(+), 95 deletions(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 1bfb7ecf0..4aa2a3087 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -220,7 +220,7 @@ def model_states_callback(self, msg: ModelStates): if i == vehicle_idx: continue - # Check if this model should be tracked as an agent + # Check if this model should be tracked as an agent or obstacle agent_type = None for prefix in self.tracked_model_prefixes: if model_name.lower().startswith(prefix.lower()): @@ -240,7 +240,7 @@ def model_states_callback(self, msg: ModelStates): break if agent_type is None and obstacle_type is None: - continue # Not an agent we're tracking + continue # Not an entity we're tracking # Get position and orientation from model states position = msg.pose[i].position @@ -253,6 +253,7 @@ 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, @@ -266,103 +267,152 @@ def model_states_callback(self, msg: ModelStates): ) # Transform agent pose to START frame using stable transformation - agent_pose = None - if self.transform_initialized: - # Calculate agent position relative to the *initial* vehicle position (from when START frame was established) - rel_x = position.x - self.initial_vehicle_model_pose.x - rel_y = position.y - self.initial_vehicle_model_pose.y - rel_z = position.z - self.initial_vehicle_model_pose.z - - # Rotate by the *initial* vehicle orientation - cos_yaw = math.cos(-self.initial_vehicle_model_pose.yaw) - sin_yaw = math.sin(-self.initial_vehicle_model_pose.yaw) - rot_x = rel_x * cos_yaw - rel_y * sin_yaw - rot_y = rel_x * sin_yaw + rel_y * cos_yaw - - # Adjust yaw relative to *initial* vehicle orientation - rel_yaw = yaw - self.initial_vehicle_model_pose.yaw - - # Create the pose in START frame using the stable transformation - agent_pose = ObjectPose( - frame=ObjectFrameEnum.START, - t=current_time - self.t_start if self.t_start is not None else 0, - x=rot_x, - y=rot_y, - z=rel_z, - roll=roll, - pitch=pitch, - yaw=rel_yaw + agent_pose = self._transform_to_start_frame(current_time, position, roll, pitch, yaw) + + # Process agent if applicable + if agent_type is not None and self.agent_detector_callback is not None: + self._process_agent(model_name, agent_type, agent_pose, position, linear_vel, angular_vel, dt) + + # Process obstacle if applicable + if obstacle_type is not None and self.obstacle_detector_callback is not None: + self._process_obstacle(model_name, obstacle_type, agent_pose, roll, pitch) + + def _transform_to_start_frame(self, current_time, position, roll, pitch, yaw): + """Transform a pose from ABSOLUTE_CARTESIAN to START frame.""" + if self.transform_initialized: + # Calculate position relative to the *initial* vehicle position (from when START frame was established) + rel_x = position.x - self.initial_vehicle_model_pose.x + rel_y = position.y - self.initial_vehicle_model_pose.y + rel_z = position.z - self.initial_vehicle_model_pose.z + + # Rotate by the *initial* vehicle orientation + cos_yaw = math.cos(-self.initial_vehicle_model_pose.yaw) + sin_yaw = math.sin(-self.initial_vehicle_model_pose.yaw) + rot_x = rel_x * cos_yaw - rel_y * sin_yaw + rot_y = rel_x * sin_yaw + rel_y * cos_yaw + + # Adjust yaw relative to *initial* vehicle orientation + rel_yaw = yaw - self.initial_vehicle_model_pose.yaw + + # Create the pose in START frame using the stable transformation + return ObjectPose( + frame=ObjectFrameEnum.START, + t=current_time - self.t_start if self.t_start is not None else 0, + x=rot_x, + y=rot_y, + z=rel_z, + roll=roll, + pitch=pitch, + yaw=rel_yaw + ) + else: + # If transformation not initialized yet, just use the global pose in ABSOLUTE_CARTESIAN frame + return ObjectPose( + frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, + t=current_time, + x=position.x, + y=position.y, + z=position.z, + roll=roll, + pitch=pitch, + yaw=yaw + ) + + def _process_agent(self, model_name, agent_type, agent_pose, position, linear_vel, angular_vel, dt): + """Process an agent detected in the simulation.""" + # 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] + + # 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: - # If transformation not initialized yet, just use the model_states pose - agent_pose = agent_global_pose + velocity = calculated_vel + + # 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 - 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 - - 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 - - # 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 + # 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) + + def _process_obstacle(self, model_name, obstacle_type, agent_pose, roll, pitch): + """Process an obstacle detected in the simulation.""" + # Determine obstacle state based on orientation + # For traffic cones, we want to check if they are standing up or tipped over + obstacle_activity = ObstacleStateEnum.STANDING # Default state + + # Check roll and pitch to determine if the obstacle is tipped over + # Thresholds in radians - approx 20 degrees + roll_threshold = 0.35 + pitch_threshold = 0.35 + + # For a traffic cone, analyze orientation + if obstacle_type == 'traffic_cone': + if abs(roll) > roll_threshold: + # Check which direction it's tipped + if roll > 0: + obstacle_activity = ObstacleStateEnum.RIGHT else: - activity = AgentActivityEnum.MOVING - - # 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) - - 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) + obstacle_activity = ObstacleStateEnum.LEFT + elif abs(pitch) > pitch_threshold: + # If tipped forward/backward, we'll use LEFT/RIGHT based on pitch sign + if pitch > 0: + obstacle_activity = ObstacleStateEnum.RIGHT + else: + obstacle_activity = ObstacleStateEnum.LEFT + + # Create obstacle state with the determined activity + obstacle_state = ObstacleState( + dimensions=(0,0,0), + outline=None, + pose=agent_pose, + type=getattr(ObstacleMaterialEnum, obstacle_type.upper()), + activity=obstacle_activity, + ) + + # Call the callback with the obstacle state + self.obstacle_detector_callback(model_name, obstacle_state) def subscribe_sensor(self, name, callback, type=None): if name == 'gnss': @@ -396,6 +446,9 @@ def callback_with_gnss_reading(inspva_msg): speed = np.linalg.norm([inspva_msg.east_velocity, inspva_msg.north_velocity]) self.last_reading.speed = speed + # Save the vehicle's GPS pose for coordinate transformation + self.vehicle_gps_pose = pose + # Create GNSS reading with fused data reading = GNSSReading( pose=pose, From 8caa9fe28a4b275113ce989ae91574ba27f555d0 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Mon, 12 May 2025 02:23:31 -0500 Subject: [PATCH 11/13] update docs --- GEMstack/onboard/interface/gem_gazebo.py | 2 +- .../perception/README_agent_detection.md | 85 ---------- .../onboard/perception/obstacle_detection.py | 4 +- docs/Gazebo Simulation Documentation.md | 53 ++++--- docs/gazebo_entity_detection.md | 147 ++++++++++++++++++ 5 files changed, 178 insertions(+), 113 deletions(-) delete mode 100644 GEMstack/onboard/perception/README_agent_detection.md create mode 100644 docs/gazebo_entity_detection.md diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 4aa2a3087..7743f2c66 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -116,7 +116,7 @@ def __init__(self): 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 + self.obstacle_detection_rate = settings.get('simulator.obstacle_tracker.rate', 10.0) # Hz # Frame transformation variables self.start_pose_abs = None # Initial vehicle pose in GLOBAL frame diff --git a/GEMstack/onboard/perception/README_agent_detection.md b/GEMstack/onboard/perception/README_agent_detection.md deleted file mode 100644 index 195db2d63..000000000 --- a/GEMstack/onboard/perception/README_agent_detection.md +++ /dev/null @@ -1,85 +0,0 @@ -# Agent Detection in GEMstack - -This module contains implementations for detecting agents (pedestrians, vehicles, etc.) in the environment. The available detectors are: - -## OmniscientAgentDetector - -The `OmniscientAgentDetector` works with the basic simulator to obtain agent states that are being simulated. It receives updates from the simulator and maintains a thread-safe dictionary of all detected agents. - -Example usage: - -```python -from onboard.perception.agent_detection import OmniscientAgentDetector -from onboard.interface.gem_simulator import GEMDoubleIntegratorSimulationInterface - -# Create the simulator interface -simulator = GEMDoubleIntegratorSimulationInterface() - -# Create the agent detector with the simulator interface -agent_detector = OmniscientAgentDetector(simulator) - -# Initialize and start -agent_detector.initialize() -simulator.start() - -# Later, when you need agent data: -agent_states = agent_detector.update() -``` - -## GazeboAgentDetector - -The `GazeboAgentDetector` works specifically with the Gazebo simulator, subscribing to the model states topic and converting Gazebo models into `AgentState` objects. It can be configured to track specific model prefixes. - -Example usage: - -```python -from onboard.perception.agent_detection import GazeboAgentDetector -from onboard.interface.gem_gazebo import GEMGazeboInterface - -# Create the Gazebo interface -gazebo_interface = GEMGazeboInterface() - -# Define the model prefixes you want to track as agents -tracked_prefixes = ['pedestrian', 'person', 'car', 'vehicle'] - -# Create the agent detector with the Gazebo interface and the tracked prefixes -agent_detector = GazeboAgentDetector(gazebo_interface, tracked_prefixes) - -# Initialize and start -agent_detector.initialize() -gazebo_interface.start() - -# Later, when you need agent data: -agent_states = agent_detector.update() -``` - -## Configuration - -The Gazebo agent detector can be configured through settings in the configuration file: - -```yaml -simulator: - agent_tracker: - model_prefixes: - - pedestrian - - person - - bicycle - - bike - - car - - vehicle - - truck - rate: 10.0 # Hz - how frequently to process model updates -``` - -The model prefixes are strings that match the beginning of Gazebo model names. For example, a model named "pedestrian1" would match the "pedestrian" prefix. - -## Agent State Format - -Both detectors provide `AgentState` objects with the following properties: - -- `pose`: The position and orientation of the agent -- `dimensions`: The physical dimensions (length, width, height) of the agent -- `type`: The type of agent (car, pedestrian, bicyclist, etc.) -- `activity`: The activity state of the agent (stopped, moving, fast) -- `velocity`: The velocity vector in the agent's local frame -- `yaw_rate`: The rate of rotation around the vertical axis \ No newline at end of file diff --git a/GEMstack/onboard/perception/obstacle_detection.py b/GEMstack/onboard/perception/obstacle_detection.py index 5654441f6..1ebefa25c 100644 --- a/GEMstack/onboard/perception/obstacle_detection.py +++ b/GEMstack/onboard/perception/obstacle_detection.py @@ -45,7 +45,7 @@ def __init__(self, vehicle_interface : GEMInterface, tracked_obstacle_prefixes=N # 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}") + print(f"Configured GazeboObstacleDetector to track obstacles with prefixes: {tracked_obstacle_prefixes}") else: print("Warning: vehicle_interface doesn't support tracked_obstacle_prefixes configuration") @@ -53,4 +53,4 @@ 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 + print("GazeboObstacleDetector initialized and subscribed to model_states") \ No newline at end of file diff --git a/docs/Gazebo Simulation Documentation.md b/docs/Gazebo Simulation Documentation.md index 88003f8ca..ba36bc9d2 100644 --- a/docs/Gazebo Simulation Documentation.md +++ b/docs/Gazebo Simulation Documentation.md @@ -99,13 +99,15 @@ GEMstack/knowledge/defaults/ - `current.yaml` - Default configuration (GEM e4) - `e2.yaml` - GEM e2 configuration -## Agent Detection in Gazebo +## Entity Detection in Gazebo -The Gazebo simulation environment supports detection of various types of agents (pedestrians, vehicles, etc.) that can be spawned in the simulation world. You can configure GEMstack to detect and track these agents. +The Gazebo simulation environment supports detection of various types of entities - both agents (pedestrians, vehicles, etc.) and obstacles (traffic cones, barriers, etc.) that can be spawned in the simulation world. -### Configuring Agent Detection in Launch File +For detailed information about entity detection, including configuration options, usage examples, and implementation details, see the [Entity Detection Documentation](gazebo_entity_detection.md). -To enable agent detection in your Gazebo simulation, add the following to your launch file's `gazebo` variant: +### Quick Start for Entity Detection + +To enable entity detection in your Gazebo simulation, add the following to your launch file's `gazebo` variant: ```yaml drive: @@ -115,35 +117,36 @@ drive: type: agent_detection.GazeboAgentDetector args: tracked_model_prefixes: ['pedestrian', 'car', 'bicycle'] + obstacle_detection: + type: obstacle_detection.GazeboObstacleDetector + args: + tracked_obstacle_prefixes: ['cone'] ``` -The `tracked_model_prefixes` parameter specifies which types of models will be tracked as agents. The prefixes listed will match against the model names in Gazebo. For example, a model named `pedestrian1` will be tracked if `pedestrian` is in the list. - -### Spawning Agents in Gazebo - -You can spawn agents in Gazebo using a YAML configuration file. - -Follow the instructions in the [POLARIS GEM Simulator](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) repository to spawn agents in Gazebo. +#### Configuration Options -### Important Naming Considerations +- **Agent Detection**: + - `type`: Specify the detector class (`agent_detection.GazeboAgentDetector`) + - `args`: Additional arguments: + - `tracked_model_prefixes`: Array of prefixes for models to track as agents + +- **Obstacle Detection**: + - `type`: Specify the detector class (`obstacle_detection.GazeboObstacleDetector`) + - `args`: Additional arguments: + - `tracked_obstacle_prefixes`: Array of prefixes for models to track as obstacles -The `name` field in your agent configuration is crucial as it determines how models are identified in Gazebo's `model_states` topic. For agent detection to work: +The prefixes in the configuration arrays define which entities will be tracked. For example: +- A model named `pedestrian1` will be detected as a pedestrian agent +- A model named `cone5` will be detected as a traffic cone obstacle -1. The model name prefix must match one of the prefixes in your `tracked_model_prefixes` configuration -2. For example, a model named `pedestrian1` will be detected as a pedestrian if `pedestrian` is in your tracked prefixes -3. The model type is derived from the prefix, so naming conventions are important +You can customize these arrays based on the entities present in your simulation environment. -When spawning models in Gazebo, ensure that their names have appropriate prefixes that match your GEMstack configuration. +### Spawning Entities in Gazebo -### How Agent Detection Works +You can spawn both agents and obstacles in Gazebo using a YAML configuration file. -The `GazeboAgentDetector` component: -1. Subscribes to Gazebo's `model_states` topic to get positions of all models -2. Filters models based on the configured `tracked_model_prefixes` -3. Transforms model positions from Gazebo's coordinate frame to GEMstack's START frame -4. Creates `AgentState` objects for each detected agent -5. Makes these agents available to other GEMstack components for planning and visualization +Follow the instructions in the [POLARIS GEM Simulator](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) repository to spawn entities in Gazebo. -The models in your simulation will appear in your GEMstack visualization and be available for planning and decision-making components to interact with. +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. --- diff --git a/docs/gazebo_entity_detection.md b/docs/gazebo_entity_detection.md new file mode 100644 index 000000000..fffc112e2 --- /dev/null +++ b/docs/gazebo_entity_detection.md @@ -0,0 +1,147 @@ +# Entity Detection in GEMstack + +This module contains implementations for detecting entities (pedestrians, vehicles, obstacles, etc.) in the environment. The available detectors are: + +## Agent Detection + +### OmniscientAgentDetector + +The `OmniscientAgentDetector` works with the basic simulator to obtain agent states that are being simulated. It receives updates from the simulator and maintains a thread-safe dictionary of all detected agents. + +Example usage: + +```python +from onboard.perception.agent_detection import OmniscientAgentDetector +from onboard.interface.gem_simulator import GEMDoubleIntegratorSimulationInterface + +# Create the simulator interface +simulator = GEMDoubleIntegratorSimulationInterface() + +# Create the agent detector with the simulator interface +agent_detector = OmniscientAgentDetector(simulator) + +# Initialize and start +agent_detector.initialize() +simulator.start() + +# Later, when you need agent data: +agent_states = agent_detector.update() +``` + +### GazeboAgentDetector + +The `GazeboAgentDetector` works specifically with the Gazebo simulator, subscribing to the model states topic and converting Gazebo models into `AgentState` objects. It can be configured to track specific model prefixes. + +Example usage: + +```python +from onboard.perception.agent_detection import GazeboAgentDetector +from onboard.interface.gem_gazebo import GEMGazeboInterface + +# Create the Gazebo interface +gazebo_interface = GEMGazeboInterface() + +# Define the model prefixes you want to track as agents +tracked_prefixes = ['pedestrian', 'person', 'car', 'vehicle'] + +# Create the agent detector with the Gazebo interface and the tracked prefixes +agent_detector = GazeboAgentDetector(gazebo_interface, tracked_prefixes) + +# Initialize and start +agent_detector.initialize() +gazebo_interface.start() + +# Later, when you need agent data: +agent_states = agent_detector.update() +``` + +### Agent State Format + +Agent detectors provide `AgentState` objects with the following properties: + +- `pose`: The position and orientation of the agent +- `dimensions`: The physical dimensions (length, width, height) of the agent +- `type`: The type of agent (car, pedestrian, bicyclist, etc.) +- `activity`: The activity state of the agent (stopped, moving, fast) +- `velocity`: The velocity vector in the agent's local frame +- `yaw_rate`: The rate of rotation around the vertical axis + +## Obstacle Detection + +### GazeboObstacleDetector + +The `GazeboObstacleDetector` works with the Gazebo simulator to detect obstacles such as traffic cones. It subscribes to the model states topic and converts Gazebo models into `ObstacleState` objects based on specific model prefixes. + +Example usage: + +```python +from onboard.perception.obstacle_detection import GazeboObstacleDetector +from onboard.interface.gem_gazebo import GEMGazeboInterface + +# Create the Gazebo interface +gazebo_interface = GEMGazeboInterface() + +# Define the model prefixes you want to track as obstacles +tracked_obstacle_prefixes = ['cone'] + +# Create the obstacle detector with the Gazebo interface +obstacle_detector = GazeboObstacleDetector(gazebo_interface, tracked_obstacle_prefixes) + +# Initialize and start +obstacle_detector.initialize() +gazebo_interface.start() + +# Later, when you need obstacle data: +obstacle_states = obstacle_detector.update() +``` + +### Obstacle State Format + +Obstacle detectors provide `ObstacleState` objects with the following properties: + +- `pose`: The position and orientation of the obstacle +- `dimensions`: The physical dimensions of the obstacle +- `type`: The material type of obstacle (traffic cone, barrier, etc.) +- `activity`: The state of the obstacle (standing, left, right) + +For traffic cones in particular, the orientation is analyzed to determine if the cone is: +- `STANDING`: Upright within normal thresholds +- `LEFT`: Tipped to the left side +- `RIGHT`: Tipped to the right side + +## Configuration + +Both detectors can be configured through settings in the configuration file: + +```yaml +simulator: + agent_tracker: + model_prefixes: + - pedestrian + - person + - bicycle + - bike + - car + - vehicle + - truck + rate: 10.0 # Hz - how frequently to process model updates + obstacle_tracker: + model_prefixes: + - cone + rate: 10.0 # Hz - how frequently to process model updates +``` + +The model prefixes are strings that match the beginning of Gazebo model names. For example, a model named "pedestrian1" would match the "pedestrian" prefix, while "cone5" would match the "cone" prefix. + +## Implementation Details + +Both agent and obstacle detection are implemented in the `GEMGazeboInterface` class, which: + +1. Monitors the Gazebo model states +2. Matches model prefixes to determine entity type +3. Transforms coordinates from Gazebo's global frame to the vehicle's START frame +4. Determines agent activity based on velocity or obstacle state based on orientation +5. Creates and maintains appropriate state objects +6. Provides detection through registered callbacks + +Both GEM e2 and GEM e4 vehicle models are supported with proper coordinate transformations. \ No newline at end of file From 788c622ad8c3d1e18ef00714bfab8527f5b9f82f Mon Sep 17 00:00:00 2001 From: shraddhaamohan Date: Tue, 13 May 2025 11:30:46 -0500 Subject: [PATCH 12/13] Resolved merge conflicts (sm148, pokkali2) --- GEMstack/onboard/interface/gem_gazebo.py | 13 +++++----- .../onboard/perception/obstacle_detection.py | 8 +++---- GEMstack/state/__init__.py | 4 ++-- GEMstack/state/obstacle.py | 24 +++++++------------ docs/gazebo_entity_detection.md | 8 +++---- 5 files changed, 25 insertions(+), 32 deletions(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 7743f2c66..2c19d60b0 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -1,4 +1,4 @@ -from GEMstack.state.obstacle import ObstacleMaterialEnum, ObstacleState, ObstacleStateEnum +from GEMstack.state.obstacle import Obstacle, ObstacleMaterialEnum, ObstacleStateEnum from .gem import * from ...utils import settings import math @@ -403,12 +403,13 @@ def _process_obstacle(self, model_name, obstacle_type, agent_pose, roll, pitch): obstacle_activity = ObstacleStateEnum.LEFT # Create obstacle state with the determined activity - obstacle_state = ObstacleState( + obstacle_state = Obstacle( dimensions=(0,0,0), outline=None, pose=agent_pose, - type=getattr(ObstacleMaterialEnum, obstacle_type.upper()), - activity=obstacle_activity, + material=getattr(ObstacleMaterialEnum, obstacle_type.upper()), + state=obstacle_activity, + collidable=True ) # Call the callback with the obstacle state @@ -556,8 +557,8 @@ def callback_with_cv2(msg: Image): 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") + if type is not None and type is not Obstacle: + raise ValueError("GEMGazeboInterface only supports Obstacle for obstacle_detector") self.obstacle_detector_callback = callback def hardware_faults(self) -> List[str]: diff --git a/GEMstack/onboard/perception/obstacle_detection.py b/GEMstack/onboard/perception/obstacle_detection.py index 1ebefa25c..97f4716c0 100644 --- a/GEMstack/onboard/perception/obstacle_detection.py +++ b/GEMstack/onboard/perception/obstacle_detection.py @@ -5,7 +5,7 @@ from ..component import Component from ..interface.gem import GEMInterface -from ...state.obstacle import ObstacleState +from ...state.obstacle import Obstacle class OmniscientObstacleDetector(Component): """Obtains agent detections from a simulator""" @@ -24,13 +24,13 @@ def state_outputs(self): return ['obstacles'] def initialize(self): - self.vehicle_interface.subscribe_sensor('obstacle_detector',self.obstacle_callback, ObstacleState) + self.vehicle_interface.subscribe_sensor('obstacle_detector',self.obstacle_callback, Obstacle) - def obstacle_callback(self, name : str, obstacle : ObstacleState): + def obstacle_callback(self, name : str, obstacle : Obstacle): with self.lock: self.obstacles[name] = obstacle - def update(self) -> Dict[str,ObstacleState]: + def update(self) -> Dict[str,Obstacle]: with self.lock: return copy.deepcopy(self.obstacles) diff --git a/GEMstack/state/__init__.py b/GEMstack/state/__init__.py index 641b67771..77d1bea71 100644 --- a/GEMstack/state/__init__.py +++ b/GEMstack/state/__init__.py @@ -8,7 +8,7 @@ 'VehicleState', 'Roadgraph', 'Roadmap', - 'Obstacle', 'ObstacleMaterialEnum','ObstacleStateEnum','ObstacleState', + 'Obstacle', 'ObstacleMaterialEnum','ObstacleStateEnum', 'Sign', 'AgentState','AgentEnum','AgentActivityEnum', 'ObstacleMaterialEnum','ObstacleStateEnum', @@ -26,7 +26,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, ObstacleStateEnum, ObstacleState +from .obstacle import Obstacle, ObstacleMaterialEnum, ObstacleStateEnum from .sign import Sign, SignEnum, SignalLightEnum, SignState from .roadmap import Roadmap from .agent import AgentState, AgentEnum, AgentActivityEnum diff --git a/GEMstack/state/obstacle.py b/GEMstack/state/obstacle.py index fb1479bb5..30a21217d 100644 --- a/GEMstack/state/obstacle.py +++ b/GEMstack/state/obstacle.py @@ -20,27 +20,19 @@ class ObstacleMaterialEnum(Enum): 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 + UNDETERMINED = 0 # unknown activity + STANDING = 1 # standing cone + LEFT = 2 # flipped cone facing left + RIGHT = 3 # flipped cone facing right @dataclass @register class Obstacle(PhysicalObject): material : ObstacleMaterialEnum collidable : bool + state: ObstacleStateEnum = ObstacleStateEnum.UNDETERMINED -@dataclass -@register -class ObstacleState(PhysicalObject): - type: ObstacleMaterialEnum - activity: ObstacleStateEnum - - def to_frame(self, frame: ObjectFrameEnum, current_pose=None, start_pose_abs=None) -> ObstacleState: + def to_frame(self, frame: ObjectFrameEnum, current_pose=None, start_pose_abs=None) -> Obstacle: 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) + return replace(self, pose=newpose) + diff --git a/docs/gazebo_entity_detection.md b/docs/gazebo_entity_detection.md index fffc112e2..ebcd6b15b 100644 --- a/docs/gazebo_entity_detection.md +++ b/docs/gazebo_entity_detection.md @@ -70,7 +70,7 @@ Agent detectors provide `AgentState` objects with the following properties: ### GazeboObstacleDetector -The `GazeboObstacleDetector` works with the Gazebo simulator to detect obstacles such as traffic cones. It subscribes to the model states topic and converts Gazebo models into `ObstacleState` objects based on specific model prefixes. +The `GazeboObstacleDetector` works with the Gazebo simulator to detect obstacles such as traffic cones. It subscribes to the model states topic and converts Gazebo models into `Obstacle` objects based on specific model prefixes. Example usage: @@ -97,12 +97,12 @@ obstacle_states = obstacle_detector.update() ### Obstacle State Format -Obstacle detectors provide `ObstacleState` objects with the following properties: +Obstacle detectors provide `Obstacle` objects with the following properties: - `pose`: The position and orientation of the obstacle - `dimensions`: The physical dimensions of the obstacle -- `type`: The material type of obstacle (traffic cone, barrier, etc.) -- `activity`: The state of the obstacle (standing, left, right) +- `material`: The material type of obstacle (traffic cone, barrier, etc.) +- `state`: The state of the obstacle (standing, left, right) For traffic cones in particular, the orientation is analyzed to determine if the cone is: - `STANDING`: Upright within normal thresholds From e3e902b722de020615c6961bb3e498e473246de7 Mon Sep 17 00:00:00 2001 From: harishkumarbalaji Date: Tue, 13 May 2025 12:09:42 -0500 Subject: [PATCH 13/13] Refactor import of ReedsSheppParking to avoid dependency error in SummoningRoutePlanner class --- GEMstack/onboard/planning/route_planning.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/GEMstack/onboard/planning/route_planning.py b/GEMstack/onboard/planning/route_planning.py index 9bae3c24a..91c33fee7 100644 --- a/GEMstack/onboard/planning/route_planning.py +++ b/GEMstack/onboard/planning/route_planning.py @@ -5,7 +5,6 @@ import os import numpy as np from .RRT import BiRRT -from .reeds_shepp_parking import ReedsSheppParking class StaticRoutePlanner(Component): @@ -216,6 +215,8 @@ class SummoningRoutePlanner(Component): """Reads a route from disk and returns it as the desired route.""" def __init__(self, roadgraphfn: str = None, map_type: str = 'roadgraph', map_frame: str = 'start'): + # Moving this import here to avoid dependency error related to reeds-shepp python package + from .reeds_shepp_parking import ReedsSheppParking self.planner = None self.route = None self.map_type = map_type