adding fake parking space detector

Author: Aadarsh

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
+from ...state import AgentState, ObjectPose, ObjectFrameEnum, Obstacle, ObstacleMaterialEnum, VehicleState, AllState
 from ..interface.gem import GEMInterface
 from .utils.detection_utils import *
 from .utils.parking_utils import *
@@ -189,4 +189,79 @@ def update(self, agents: Dict[str, AgentState]):
                     )
 
         new_state = [goal_pose, parking_obstacles]
-        return new_state
+        return new_state
+    
+
+class FakeParkingSpaceDetector(Component):
+    def __init__(self, vehicle_interface: GEMInterface):
+        self.vehicle_interface = vehicle_interface
+        
+        # Hard-coded goal position for the parking spot
+        self.goal_parking_spot = (8.0, 20.0, 0.0)  # (x, y, yaw)
+
+        # Hard-coded obstacles blocking the side paths only (not the mouth of the parking)
+        self.parking_obstacles_pose = [
+            (6.0, 18.0, 0.0, 0.0),  # Left obstacle (x, y, z, yaw)
+            (10.0, 18.0, 0.0, 0.0)  # Right obstacle (x, y, z, yaw)
+        ]
+
+        # Obstacle dimensions (width, length, height)
+        self.parking_obstacles_dim = [
+            [0.5, 0.5, 1.0],
+            [0.5, 0.5, 1.0]
+        ]
+
+    def rate(self):
+        return 4.0
+
+    def state_inputs(self):
+        return ['agents']
+
+    def state_outputs(self):
+        return ['goal', 'obstacles']
+
+    def update(self, state: AllState):
+        # Current timestamp
+        current_time = self.vehicle_interface.time()
+
+        # Constructing goal pose
+        x, y, yaw = self.goal_parking_spot
+        goal_pose = ObjectPose(
+            t=current_time,
+            x=x,
+            y=y,
+            z=0.0,
+            yaw=yaw,
+            pitch=0.0,
+            roll=0.0,
+            frame=ObjectFrameEnum.CURRENT
+        )
+
+        # Constructing obstacles for only the two side walls
+        obstacle_id = 0
+        parking_obstacles = {}
+        for o_pose, o_dim in zip(self.parking_obstacles_pose, self.parking_obstacles_dim):
+            x, y, z, yaw = o_pose
+            obstacle_pose = ObjectPose(
+                t=current_time,
+                x=x,
+                y=y,
+                z=z,
+                yaw=yaw,
+                pitch=0.0,
+                roll=0.0,
+                frame=ObjectFrameEnum.START
+            )
+            new_obstacle = Obstacle(
+                pose=obstacle_pose,
+                dimensions=o_dim,
+                outline=None,
+                material=ObstacleMaterialEnum.BARRIER,
+                collidable=True
+            )
+            parking_obstacles[f"parking_obstacle_{obstacle_id}"] = new_obstacle
+            obstacle_id += 1
+
+        # Returning the hard-coded goal and obstacles
+        new_state = [goal_pose, parking_obstacles]
+        return new_state

launch/parking_detection.yaml

@@ -92,7 +92,7 @@ variants:
                     # route_planning_component:
                     #     type: RoutePlanningComponent
 
-    sim: # full pipeline
+    sim: # full pipeline, sim, real perception
         run:
             mode: simulation
             vehicle_interface:
@@ -178,6 +178,48 @@ variants:
                     trajectory_tracking:
                         type: pure_pursuit.PurePursuitTrajectoryTracker
                         print: False
+    
+    real_sim: # real on-board planning w/ fake perception
+        run:
+            mode: hardware
+            vehicle_interface:
+                type: gem_hardware.GEMHardwareInterface
+
+            drive: 
+                perception:
+                    state_estimation : GNSSStateEstimator
+                    agent_detection : OmniscientAgentDetector
+                    parking_detection: parking_detection.FakeParkingSpaceDetector
+                        
+                    
+                planning:
+                    parking_component:
+                        type: ParkingSim
+                    route_planning_component:
+                        type: RoutePlanningComponent
+                        dubins: 
+                            integrator:
+                                timestep: 1
+                                dt: .25
+                                max_velocity: 0.75
+                                max_turning_rate: 0.3
+                                tolerance: 0.25
+                        astar:
+                            iterations: 200 # We should have some sort of max iterations/timeout
+                            heuristic: reeds_shepp
+
+                    motion_planning:
+                        type: longitudinal_planning.YieldTrajectoryPlanner
+                        # type: yield_spline_planner.SplinePlanner
+                        largs:
+                            acceleration: 1
+                            deceleration: 2
+                            desired_speed: 1
+
+                    trajectory_tracking:
+                        type: pure_pursuit.PurePursuitTrajectoryTracker
+                        print: False
+
     log_ros:
         log:
             ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml"