Gazebo Entity detection - Agent and Obstacle Detection and Tracking - Feature Request

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")

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 Obstacle
+
+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, Obstacle)
+
+    def obstacle_callback(self, name : str, obstacle : Obstacle):
+        with self.lock:
+            self.obstacles[name] = obstacle
+
+    def update(self) -> Dict[str,Obstacle]:
+        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 GazeboObstacleDetector 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("GazeboObstacleDetector initialized and subscribed to model_states")

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

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,44 +94,177 @@ def update(self, state):
         self.debug("vehicle", "velocity", state.vehicle.v)
         self.debug("vehicle", "front_wheel_angle", state.vehicle.front_wheel_angle)
 
-        # Pedestrian metrics
+        # 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 = []
         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
-                self.debug(f"ped_{agent_id}", "yaw_rate", agent.yaw_rate)
+            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
+
+                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))
-        #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}")
+
+        # Plot using the current state directly - avoid conversions that might fail
+        try:
+            # 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])
+
+                # Just draw dots for vehicle and agents
+                self.axs[0].plot(state.vehicle.pose.x, state.vehicle.pose.y, 'bo', markersize=10, label='Vehicle')
+
+                # 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)}")
+
+        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)}")
 
-        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:

GEMstack/state/__init__.py

@@ -8,7 +8,7 @@
            'VehicleState',
            'Roadgraph',
            'Roadmap',
-           'Obstacle',
+           'Obstacle', 'ObstacleMaterialEnum','ObstacleStateEnum',
            'Sign',
            'AgentState','AgentEnum','AgentActivityEnum',
            'ObstacleMaterialEnum','ObstacleStateEnum',

GEMstack/state/obstacle.py

@@ -1,7 +1,8 @@
 from __future__ import annotations
 from dataclasses import dataclass, replace
 from ..utils.serialization import register
-from .physical_object import ObjectFrameEnum,ObjectPose,PhysicalObject,convert_vector
+from .physical_object import ObjectFrameEnum,PhysicalObject #,convert_vector
+
 from enum import Enum
 from typing import Tuple
 
@@ -24,11 +25,14 @@ class ObstacleStateEnum(Enum):
     LEFT = 2            # flipped cone facing left
     RIGHT = 3           # flipped cone facing right
 
-
-
 @dataclass
 @register
 class Obstacle(PhysicalObject):
     material : ObstacleMaterialEnum
     collidable : bool
-    state: ObstacleStateEnum = ObstacleStateEnum.UNKNOWN
+    state: ObstacleStateEnum = ObstacleStateEnum.UNDETERMINED
+
+    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)
+        return replace(self, pose=newpose)
+