Merge parking integration with all the metrics function

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 *
@@ -135,12 +135,8 @@ def update(self, agents: Dict[str, AgentState]):
             cone_pts_3D.append(cone_pt_3D)
 
         # Detect parking spot
-        if len(cone_pts_3D) == 4:
+        if len(cone_pts_3D) >= 4:
             goal_parking_spot, parking_obstacles_pose, parking_obstacles_dim, ordered_cone_ground_centers_2D = detect_parking_spot(cone_pts_3D)
-        elif len(cone_pts_3D) > 4 and len(cone_pts_3D) % 2 == 0:
-            ordered_centroids = sorted(cone_pts_3D, key=lambda x: (x[0]))
-            ordered_centroids = [ordered_centroids[i] for i in [0, 1, 2, 4]]
-            goal_parking_spot, parking_obstacles_pose, parking_obstacles_dim, ordered_cone_ground_centers_2D = detect_parking_spot(ordered_centroids)
         # Update local variables for visualization
         self.cone_pts_3D = cone_pts_3D
         self.ordered_cone_ground_centers_2D = ordered_cone_ground_centers_2D
@@ -166,7 +162,7 @@ def update(self, agents: Dict[str, AgentState]):
                                 yaw=yaw,
                                 pitch=0.0,
                                 roll=0.0,
-                                frame=ObjectFrameEnum.CURRENT
+                                frame=ObjectFrameEnum.START
                             )
             new_obstacle = Obstacle(
                                 pose=obstacle_pose,
@@ -193,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

GEMstack/onboard/planning/longitudinal_planning.py

@@ -4,7 +4,7 @@
 from ..component import Component
 from ...state import AllState, VehicleState, EntityRelation, EntityRelationEnum, Path, Trajectory, Route, \
     ObjectFrameEnum
-from ...utils import serialization
+from ...utils import serialization, settings
 from ...mathutils import transforms
 import numpy as np
 
@@ -378,10 +378,10 @@ class YieldTrajectoryPlanner(Component):
     def __init__(self):
         self.route_progress = None
         self.t_last = None
-        self.acceleration = 5
-        self.desired_speed = 2.0
-        self.deceleration = 2.0
-        self.emergency_brake = 8.0
+        self.acceleration = settings.get("run.drive.planning.motion_planning.largs.acceleration", 5)
+        self.desired_speed = settings.get("run.drive.planning.motion_planning.largs.desired_speed",2.0)
+        self.deceleration = settings.get("run.drive.planning.motion_planning.largs.deceleration", 2.0)
+        self.emergency_brake = settings.get("run.drive.planning.motion_planning.largs.emergency_brake", 8.0)
 
     def state_inputs(self):
         return ['all']

GEMstack/onboard/planning/parking_route_planner.py

@@ -220,10 +220,12 @@ def __init__(self, vehicle=None, obstacles=None, actions=None):
         self._vehicle = None
 
         # settings
-        self.T = settings.get("planning.dubins.integrator.time_step", 1.5)
-        self.dt = settings.get("planning.dubins.integrator.time_step", .25)
-        self.max_v = settings.get("planning.dubins.actions.max_velocity", 0.75)
-        self.max_turn_rate = settings.get("planning.dubins.actions.max_turn_rate", 0.3)
+        self.T = settings.get("run.drive.planning.route_planning_component.dubins.integrator.time_step", 1.5)
+        self.dt = settings.get("run.drive.planning.route_planning_component.dubins.integrator.dt", .25)
+        self.max_v = settings.get("run.drive.planning.route_planning_component.dubins.actions.max_velocity", 0.75)
+        self.max_turn_rate = settings.get("run.drive.planning.route_planning_component.dubins.actions.max_turn_rate", 0.3)
+        self.tolerance = settings.get("run.drive.planning.route_planning_component.dubins.tolerance", 0.5)
+        self.heuristic_string = settings.get("run.drive.planning.route_planning_component.astar.heuristic", "reeds_shepp")
 
         self.vehicle = DubinsCar() #x = (tx,ty,theta) and u = (fwd_velocity,turnRate).
         self.vehicle_sim = DubinsCarIntegrator(self.vehicle, self.T, self.dt)
@@ -260,19 +262,24 @@ def is_goal_reached(self, current: List[float], goal: List[float]):
         # @TODO Currently, the threshold is just a random number, get rid of magic constants
         # print(f"Current Pose: {current}")
         # print(f"Goal Pose: {goal}")
-        # if np.abs(current[3]) > 1: return False # car must be stopped, this equality will only work in simulation  
+        # if np.abs(current[3]) > 1: return False # car must be stopped, this equality will only work in simulation np.linalg.norm(np.array([current[0], current[1]]) - np.array([goal[0], goal[1]])) < 0.5
+        # if np.abs(current[3] - goal[3]) > .25: return False 
         return np.linalg.norm(np.array([current[0], current[1]]) - np.array([goal[0], goal[1]])) < 0.5
 
     def heuristic_cost_estimate(self, state_1, state_2):
-        """computes the 'direct' distance between two (x,y) tuples"""
-        # Extract position and orientation from states
-        (x1, y1, theta1, t) = state_1
-        (x2, y2, theta2, t) = state_2
-
-        return self.reed_shepp(state_1, state_2) # Using turning radius of 1.0
+        """run the correct heuristic function based on the heuristic_string"""
 
-        return self.approx_reeds_shepp(state_1, state_2) # Using turning radius of 1.0
-        return math.hypot(x2 - x1, y2 - y1, theta2 - theta1)
+        if self.heuristic_string == "reeds_shepp":
+            return self.reed_shepp(state_1, state_2) # Using turning radius of 1.0
+
+        elif self.heuristic_string == "approx_reeds_shepp":
+            return self.approx_reeds_shepp(state_1, state_2) # Using turning radius of 1.0
+
+        elif self.heuristic_string == "euclidian":
+            return self.euclidian_cost_esimate(state_1, state_2)
+
+        else:
+            raise Exception(f"Unknown heuristic function: {self.heuristic_string}")
 
     def euclidian_cost_esimate(self, state_1, state_2):
         """computes the 'direct' distance between two (x,y) tuples"""
@@ -399,7 +406,7 @@ def state_to_object(self, state):
         theta = state[2]
         t = state[3]
 
-        pose = ObjectPose(frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN,t=t, x=x,y=y,z=0,yaw=theta)
+        pose = ObjectPose(frame=ObjectFrameEnum.CURRENT,t=t, x=x,y=y,z=0,yaw=theta)
 
         temp_obj = PhysicalObject(pose=pose,
                                dimensions=self.vehicle.to_object().dimensions,
@@ -432,7 +439,7 @@ def __init__(self):
         # self.planner = ParkingSolverSecondOrderDubins()
         self.planner = ParkingSolverFirstOrderDubins()
 
-        self.iterations = settings.get("planning.astar.iterations", 20000)
+        self.iterations = settings.get("run.drive.planning.route_planning_component.astar.iterations", 50000)
 
     def state_inputs(self):
         return ['all']
@@ -452,6 +459,7 @@ def vehicle_state_to_second_order(self, vehicle_state: VehicleState) -> Tuple[fl
         Returns:
             Tuple[float, float]: _description_
         """
+        vehicle_state.pose = vehicle_state.pose.to_frame()
         x = vehicle_state.pose.x
         y = vehicle_state.pose.y
         theta = vehicle_state.pose.yaw # check that this is correct
@@ -478,6 +486,22 @@ def vehicle_state_to_first_order(self, vehicle_state: VehicleState) -> Tuple[flo
         t = 0
 
         return (x,y,theta,t)
+
+    def pose_to_first_order(self, pose: ObjectPose) -> Tuple[float, float]:
+        """Takes a vehicle state and outputs the state of a second order dubins car
+
+        Args:
+            vehicle_state (VehicleState): _description_
+
+        Returns:
+            Tuple[float, float]: _description_
+        """
+        x = pose.x
+        y = pose.y
+        theta = pose.yaw # check that this is correct
+        t = 0
+
+        return (x,y,theta,t)
 
     def update(self, state : AllState) -> Route:
         """_summary_
@@ -495,30 +519,36 @@ def update(self, state : AllState) -> Route:
         all_obstacles = {**agents, **obstacles}
         # print(f"Obstacles {obstacles}")
         print(f"Agents {agents}")
-        # goal= state.goal
-        # print(goal.frame)
-        # assert goal.frame == ObjectFrameEnum.ABSOLUTE_CARTESIAN
-        # assert goal.v == 0
-        # print(f"Goal {goal}")
-        goal_pose = ObjectPose(frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN, t=0.0, x=state.mission_plan.goal_x,y=state.mission_plan.goal_y,z=0.0,yaw=state.mission_plan.goal_orientation)
-        goal = VehicleState.zero()
-        goal.pose = goal_pose
-        goal.v = 0
+        goal_pose = state.goal
+        assert goal_pose.frame == ObjectFrameEnum.CURRENT
+        print("Checking VALUE: ", state.start_vehicle_pose)
+        print("Checking VALUE: ", state.vehicle)
+        start_state = state.vehicle.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose)
+        # start_pose = state.vehicle.pose.to_frame(ObjectFrameEnum.CURRENT, current_pose = state.vehicle.pose,start_pose_abs = state.start_vehicle_pose)
+        # start_cur = VehicleState.zero()
+        # start_cur.pose = start_pose              # goal.pose = goal_pose
+        # goal.v = 0  
+        print("Checking VALUE: ", start_state)
+
+
+
 
         # # Need to parse and create second order dubin car states
         # start_state = self.vehicle_state_to_dynamics(vehicle)
         # goal_state = self.vehicle_state_to_dynamics(goal)
 
         # Need to parse and create second order dubin car states
-        start_state = self.vehicle_state_to_first_order(vehicle)
-        goal_state = self.vehicle_state_to_first_order(goal)
+        start = self.vehicle_state_to_first_order(start_state)
+        # goal = self.vehicle_state_to_first_order(goal_state)
+        goal = self.pose_to_first_order(goal_pose)
 
         # Update the planner
         self.planner.obstacles = list(all_obstacles.values())
         self.planner.vehicle = vehicle
 
         # Compute the new trajectory and return it 
-        res = list(self.planner.astar(start_state, goal_state, reversePath=False, iterations=self.iterations))
+        print(f"Iteartions ------------------------------ {self.iterations}")
+        res = list(self.planner.astar(start, goal, reversePath=False, iterations=self.iterations))
         # points = [state[:2] for state in res] # change here to return the theta as well
         points = []
         for state in res:

GEMstack/onboard/planning/route_planning_component.py

@@ -12,7 +12,7 @@
 from .parking_route_planner import ParkingPlanner
 from .parking_scanning import StraightLineMotion
 from .longitudinal_planning import longitudinal_plan
-
+import time
 
 
 class RoutePlanningComponent(Component):
@@ -65,6 +65,8 @@ def update(self, state: AllState):
             desired_points = [(state.vehicle.pose.x, state.vehicle.pose.y),
                               (state.vehicle.pose.x, state.vehicle.pose.y)]
             desired_path = Path(state.vehicle.pose.frame, desired_points)
+
+            desired_path = desired_path.to_frame(state.vehicle.pose.frame, current_pose=state.vehicle.pose, start_pose_abs=state.start_vehicle_pose)
 
             self.compute_parking_route = True
 
@@ -75,9 +77,14 @@ def update(self, state: AllState):
             if not self.already_computed:
                 self.planner = ParkingPlanner()
                 self.done_computing = True
+                time_before = time.time()
                 self.route = self.planner.update(state)
+                time_after = time.time()
+                print("COMPUTE TIME:", time_after - time_before)
+                self.route = self.route.to_frame(ObjectFrameEnum.START, current_pose=state.vehicle.pose, start_pose_abs=state.start_vehicle_pose)
                 self.planner.visualize_trajectory(self.route)
                 self.already_computed = True
+
             return self.route
         elif state.mission_plan.planner_type.name == "RRT_STAR":
             print("I am in RRT mode")
@@ -93,9 +100,10 @@ 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(state.vehicle.pose.frame, desired_points)
+            desired_path = Path(ObjectFrameEnum.CURRENT, 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)
             return desired_path
             # self.planner.update_speed()