working

Author: Aadarsh

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -22,7 +22,7 @@ components:
       inputs: [vehicle, roadgraph]
       outputs: vehicle_lane
   - parking_detection:
-      inputs: agents
+      inputs: obstacles
       outputs: [goal, obstacles]
   - sign_detection: 
       inputs: [vehicle, roadgraph]
@@ -49,13 +49,10 @@ components:
   - driving_logic:
       inputs: 
       outputs: intent
-  - parking_component: # one way
+  - _mission_planner: # one way
       inputs: all
       outputs: mission_plan
-  - summon_component: # one way
-      inputs: all
-      outputs: route
-  - route_planning_component: # one way
+  - _route_planner: # one way
       inputs: all
       outputs: route
   - motion_planning:
@@ -66,4 +63,4 @@ components:
       outputs: 
   - signaling:
       inputs: [intent]
-      outputs:
+      outputs:

GEMstack/onboard/perception/cone_detection.py

@@ -1,4 +1,4 @@
-from ...state import AllState, VehicleState, ObjectPose, ObjectFrameEnum, AgentState, AgentEnum, AgentActivityEnum
+from ...state import AllState, VehicleState, ObjectPose, ObjectFrameEnum, AgentState, AgentEnum, AgentActivityEnum, ObstacleState, Obstacle, ObstacleStateEnum, ObstacleMaterialEnum
 from ..interface.gem import GEMInterface
 from ..component import Component
 from ultralytics import YOLO
@@ -707,9 +707,12 @@ def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
 
 class FakConeDetector(Component):
     def __init__(self, vehicle_interface: GEMInterface):
+        """
+        Initializes the FakeConeDetector with two parking spots:
+        - One standard 2x2 rectangular spot
+        - One wider mouth for easier entry
+        """
         self.vehicle_interface = vehicle_interface
-        self.times = [(5.0, 20.0), (30.0, 35.0)]
-        self.t_start = None
 
     def rate(self):
         return 4.0
@@ -718,27 +721,71 @@ def state_inputs(self):
         return ['vehicle']
 
     def state_outputs(self):
-        return ['agents']
+        return ['obstacles']
 
-    def update(self, vehicle: VehicleState) -> Dict[str, AgentState]:
-        if self.t_start is None:
-            self.t_start = self.vehicle_interface.time()
-        t = self.vehicle_interface.time() - self.t_start
+    def update(self, vehicle: VehicleState) -> Dict[str, ObstacleState]:
+        """
+        Called every simulation step, updates the timestamp and publishes the obstacle states.
+        """
+
+        current_time = self.vehicle_interface.time()
+        
+        # === Standard Parking Spot ===
+        # cones_standard = {
+        #     'cone0': (5.0, 5.0, 0.5, 0.5),   # Front Left
+        #     'cone1': (5.0, 7.0, 0.5, 0.5),   # Back Left
+        #     'cone2': (7.0, 5.0, 0.5, 0.5),   # Front Right
+        #     'cone3': (7.0, 7.0, 0.5, 0.5)    # Back Right
+        # }
+
+        # === Wider Mouth Parking Spot ===
+        cones_wide = {
+            'cone4': (6.0, 9.0, 0.5, 0.5),  # Front Left (wide)
+            'cone5': (10.0, 9.0, 0.5, 0.5),  # Back Left
+            'cone6': (3.0, 4.0, 0.5, 0.5),  # Front Right (wide)
+            'cone7': (7.0, 4.0, 0.5, 0.5)   # Back Right
+        }
+
+        # Populate the obstacle states
         res = {}
-        for time_range in self.times:
-            if t >= time_range[0] and t <= time_range[1]:
-                res['cone0'] = box_to_fake_agent((0, 0, 0, 0))
-                rospy.loginfo("Detected a Cone (simulated)")
-        return res
+        for name, box in {**cones_wide}.items():
+            res[name] = box_to_fake_obstacle(box, current_time)
+
+        # Update timestamp
+        for obstacle in res.values():
+            obstacle.pose.t = current_time
 
+        rospy.loginfo(f"[FakeConeDetector] Simulated Two Parking Spots Detected")
+        return res
 
-def box_to_fake_agent(box):
+def box_to_fake_obstacle(box, current_time):
+    """
+    Helper function to create a fake obstacle (cone) from bounding box coordinates.
+    """
     x, y, w, h = box
-    pose = ObjectPose(t=0, x=x + w / 2, y=y + h / 2, z=0, yaw=0, pitch=0, roll=0, frame=ObjectFrameEnum.CURRENT)
-    dims = (w, h, 0)
-    return AgentState(pose=pose, dimensions=dims, outline=None,
-                      type=AgentEnum.CONE, activity=AgentActivityEnum.MOVING,
-                      velocity=(0, 0, 0), yaw_rate=0)
+    pose = ObjectPose(
+        t=current_time,
+        x=x,
+        y=y,
+        z=0.0,
+        yaw=0.0,
+        pitch=0.0,
+        roll=0.0,
+        frame=ObjectFrameEnum.START
+    )
+
+    dims = (w, h, 1.0)
+
+    new_obstacle = Obstacle(
+                                pose=pose,
+                                dimensions=dims,
+                                outline=None,
+                                material=ObstacleMaterialEnum.TRAFFIC_CONE,
+                                collidable=True
+                            )
+    print("Obstacles made")
+    return new_obstacle
+
 
 
 if __name__ == '__main__':

GEMstack/onboard/perception/fake_obstacle_detection.py

@@ -0,0 +1,97 @@
+from ...state import VehicleState, ObjectPose, ObjectFrameEnum, ObstacleState, ObstacleMaterialEnum, ObstacleStateEnum
+from ..interface.gem import GEMInterface
+from ..component import Component
+from typing import Dict
+import yaml
+import rospy
+import time
+from ...utils import settings
+
+
+OBSTACLE_TYPE_TO_ENUM = {
+    'cone': ObstacleMaterialEnum.TRAFFIC_CONE,
+}
+
+OBSTACLE_DIMENSIONS = {
+    'cone': (0.5, 0.5, 1.0),
+}
+
+
+class FakeObstacleDetector(Component):
+    def __init__(self, vehicle_interface: GEMInterface):
+        """
+        Args:
+            vehicle_interface: GEMInterface instance
+        """
+        self.vehicle_interface = vehicle_interface
+        
+        # Load path from the settings, with a default if not present
+        self.scene_path = settings.get("run.drive.perception.obstacle_detection.largs.scene_path", None)
+
+        if not self.scene_path:
+            raise ValueError("Scene path not specified in the YAML configuration under `largs.scene_path`")
+        
+        self.t_start = None
+        self.obstacles = self.load_obstacles(self.scene_path)
+
+    def load_obstacles(self, scene_path: str) -> Dict[str, ObstacleState]:
+        """
+        Loads obstacles from a YAML scene file and initializes them as ObstacleState objects.
+        """
+        print(f"[FakeObstacleDetector] Loading obstacles from: {scene_path}")
+        
+        with open(scene_path, 'r') as file:
+            data = yaml.safe_load(file)
+
+        obstacles = {}
+        for name, attributes in data['obstacles'].items():
+            pose = ObjectPose(
+                frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN,
+                t=0.0,  # Time is dynamically assigned during simulation
+                x=attributes['position'][0],
+                y=attributes['position'][1],
+                yaw=attributes.get('yaw', 0)
+            )
+
+            # Add velocity if it exists, else default to (0, 0, 0)
+
+            obstacle_state = ObstacleState(
+                type=OBSTACLE_TYPE_TO_ENUM[attributes['type']],
+                activity=ObstacleStateEnum.STANDING,
+                pose=pose,
+                dimensions=OBSTACLE_DIMENSIONS[attributes['type']],
+                outline=None
+            )
+
+            # Add the velocity attribute directly
+
+            obstacles[name] = obstacle_state
+        
+        return obstacles
+
+    def rate(self):
+        return 4.0
+
+    def state_inputs(self):
+        return ['vehicle']
+
+    def state_outputs(self):
+        return ['obstacles']
+
+    def update(self, vehicle: VehicleState) -> Dict[str, ObstacleState]:
+        """
+        Called every simulation step, updates the timestamp and publishes the obstacle states.
+        """
+        if self.t_start is None:
+            self.t_start = self.vehicle_interface.time()
+        
+        t = self.vehicle_interface.time() - self.t_start
+        
+        # Update all obstacle poses with the current simulation time
+        res = {}
+        for name, obstacle in self.obstacles.items():
+            obstacle.pose.t = t
+            res[name] = obstacle
+            rospy.loginfo(f"[FakeObstacleDetector] Simulated Obstacle Detected: {name} at {obstacle.pose.x}, {obstacle.pose.y}")
+        
+        return res

GEMstack/onboard/perception/parking_detection.py

@@ -3,7 +3,7 @@
 from sensor_msgs.msg import PointCloud2
 from typing import Dict
 from ..component import Component 
-from ...state import AgentState, ObjectPose, ObjectFrameEnum, Obstacle, ObstacleMaterialEnum, VehicleState, AllState
+from ...state import AgentState, ObjectPose, ObjectFrameEnum, Obstacle, ObstacleMaterialEnum, VehicleState, AllState, ObstacleState
 from ..interface.gem import GEMInterface
 from .utils.detection_utils import *
 from .utils.parking_utils import *
@@ -60,7 +60,7 @@ def rate(self) -> float:
         return 10.0  # Hz
 
     def state_inputs(self) -> list:
-        return ['agents']
+        return ['obstacles']
 
     def state_outputs(self) -> list:
         return ['goal', 'obstacles']
@@ -121,7 +121,7 @@ def visualize(self,
             self.pub_cones_centers_pc2.publish(ros_cones_centers_pc2)
 
 
-    def update(self, agents: Dict[str, AgentState]):
+    def update(self, agents: Dict[str, ObstacleState]):
         # Initial variables
         goal_parking_spot = None
         parking_obstacles_pose = []
@@ -154,6 +154,7 @@ def update(self, agents: Dict[str, AgentState]):
         parking_obstacles = {}
         for o_pose, o_dim in zip(parking_obstacles_pose, parking_obstacles_dim):
             x, y, z, yaw = o_pose
+            print("CONSTRUCTING OBSTACLE POSE")
             obstacle_pose = ObjectPose(
                                 t=current_time,
                                 x=x,

GEMstack/onboard/planning/route_planning_component.py

@@ -100,7 +100,7 @@ def update(self, state: AllState):
             print("I am in SCANNING mode")
             desired_points = [(state.vehicle.pose.x, state.vehicle.pose.y),
                               (state.vehicle.pose.x + 1, state.vehicle.pose.y)]
-            desired_path = Path(ObjectFrameEnum.CURRENT, desired_points)
+            desired_path = Path(ObjectFrameEnum.START, desired_points)
 
             # @TODO these are constants we need to get from settings
             desired_path = desired_path.to_frame(state.vehicle.pose.frame, current_pose=state.vehicle.pose, start_pose_abs=state.start_vehicle_pose)

GEMstack/state/__init__.py

@@ -12,6 +12,7 @@
            'Sign',
            'AgentState','AgentEnum','AgentActivityEnum',
            'SceneState',
+           'ObstacleState','ObstacleMaterialEnum','ObstacleStateEnum',
            'VehicleIntent','VehicleIntentEnum',
            'AgentIntent',
            'EntityRelationEnum','EntityRelation','EntityRelationGraph',
@@ -24,6 +25,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, ObstacleState, ObstacleStateEnum
 from .obstacle import Obstacle, ObstacleMaterialEnum
 from .sign import Sign, SignEnum, SignalLightEnum, SignState
 from .roadmap import Roadmap
@@ -34,6 +36,6 @@
 from .relations import EntityRelation, EntityRelationEnum, EntityRelationGraph
 from .mission import MissionEnum,MissionObjective
 from .route import Route, PlannerEnum
-from .mission_plan import MissionPlan
+from .mission import MissionPlan
 from .predicates import PredicateValues
-from .all import AllState
+from .all import AllState

GEMstack/state/all.py

@@ -7,7 +7,7 @@
 from .agent_intent import AgentIntent,AgentIntentMixture
 from .relations import EntityRelation
 from .mission import MissionObjective
-from .mission_plan import MissionPlan
+from .mission import MissionPlan
 from .route import Route
 from .trajectory import Trajectory
 from .predicates import PredicateValues

GEMstack/state/mission.py

@@ -1,6 +1,8 @@
 from dataclasses import dataclass
 from ..utils.serialization import register
 from enum import Enum
+from .route import PlannerEnum
+
 
 class MissionEnum(Enum):
     IDLE = 0            # not driving, no mission
@@ -14,4 +16,15 @@ class MissionEnum(Enum):
 @register
 class MissionObjective:
     type : MissionEnum = MissionEnum.IDLE
+    
+@dataclass
+@register
+class MissionPlan:
+    goal_x: float
+    goal_y: float
+    goal_orientation: float
+    planner_type : PlannerEnum = PlannerEnum.RRT_STAR
+    # other mission-specific parameters can be added here
+
+