Adding new environments

Author: SimonKato

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"""
@@ -432,7 +439,7 @@ def __init__(self):
         # self.planner = ParkingSolverSecondOrderDubins()
         self.planner = ParkingSolverFirstOrderDubins()
 
-        self.iterations = settings.get("planning.astar.iterations", 200)
+        self.iterations = settings.get("run.drive.planning.route_planning_component.astar.iterations", 50000)
 
     def state_inputs(self):
         return ['all']
@@ -479,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_
@@ -496,34 +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.CURRENT, t=15, x=state.mission_plan.goal_x,y=state.mission_plan.goal_y,z=0,yaw=state.mission_plan.goal_orientation)
+        goal_pose = state.goal
+        assert goal_pose.frame == ObjectFrameEnum.CURRENT
         print("Checking VALUE: ", 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 = VehicleState.zero()
-        goal.pose = goal_pose
-        goal.v = 0
+        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(start_cur)
-        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:

launch/parking_detection.yaml

@@ -91,7 +91,7 @@ variants:
             vehicle_interface:
                 type: gem_simulator.GEMDoubleIntegratorSimulationInterface
                 args:
-                    scene: !relative_path '../scenes/parking_demo.yaml'
+                    scene: !relative_path '../scenes/parking/demo_2.yaml'
             # visualization: !include "klampt_visualization.yaml"
 
             drive: 
@@ -130,7 +130,7 @@ variants:
             vehicle_interface:
                 type: gem_simulator.GEMDoubleIntegratorSimulationInterface
                 args:
-                    scene: !relative_path '../scenes/parking_demo.yaml'
+                    scene: !relative_path '../scenes/parking/demo_3.yaml'
             visualization: !include "klampt_visualization.yaml"
 
             drive: 
@@ -145,10 +145,28 @@ variants:
                         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: 5000 # We should have some sort of max iterations/timeout
+                            heuristic: reeds_shepp
+
+
                     # route_planning:
                     #     type: StaticRoutePlanner
                     #     args: [!relative_path '../GEMstack/knowledge/routes/forward_15m.csv'] #forward_15m
-                    motion_planning: longitudinal_planning.YieldTrajectoryPlanner
+                    motion_planning:
+                        type: longitudinal_planning.YieldTrajectoryPlanner
+                        largs:
+                            acceleration: 1
+                            deceleration: 2
+                            desired_speed: 1
+
                     trajectory_tracking:
                         type: pure_pursuit.PurePursuitTrajectoryTracker
                         print: False

scenes/parking/demo_1.yaml

@@ -0,0 +1,33 @@
+vehicle_state: [5.0, 5.0, 0.0, 0.0, 0.0]
+agents:
+  cone1:
+    type: cone
+    position: [50.0, 8.0]
+    nominal_velocity: 0.0
+    target: [60.0, 8.0]
+    behavior: loop
+    yaw: 0
+
+  cone2:
+    type: cone
+    position: [56.0, 8.0]
+    nominal_velocity: 0.0
+    target: [60.0, 8.0]
+    behavior: loop
+    yaw: 0
+
+  cone3:
+    type: cone
+    position: [50.0, 4.0]
+    nominal_velocity: 0.0
+    target: [60.0, 8.0]
+    behavior: loop
+    yaw: 0
+
+  cone4:
+    type: cone
+    position: [56.0, 4.0]
+    nominal_velocity: 0.0
+    target: [60.0, 8.0]
+    behavior: loop
+    yaw: 0

scenes/parking/demo_10.yaml

@@ -0,0 +1,19 @@
+vehicle_state: [0.0, 0.0, 0.0, 0.0, 0.0]
+agents:
+  ped1:
+    type: medium_truck
+    position: [2.5, 8.0]
+    nominal_velocity: 0.0
+    target: [20.0, 8.0]
+    behavior: loop
+    yaw: 1.5708
+
+  ped2:
+    type: medium_truck
+    position: [7.5, 8.0]
+    nominal_velocity: 0.0
+    target: [15.0, 4.0]
+    behavior: loop
+    yaw: 1.5708
+
+    

scenes/parking_demo.yaml scenes/parking/demo_2.yamlscenes/parking_demo.yaml renamed to scenes/parking/demo_2.yaml

@@ -1,49 +1,50 @@
-vehicle_state: [5.0, 5.0, 0.0, 0.0, 0.0]
-agents:
-  # ped1:
-  #   type: medium_truck
-  #   position: [20.0, 8.0]
-  #   nominal_velocity: 0.0
-  #   target: [20.0, 8.0]
-  #   behavior: loop
-  #   yaw: 1.5708
+vehicle_state: [-1.0, -2.0, 0.0, 0.0, 0.0]
 
-  # ped2:
-  #   type: medium_truck
-  #   position: [30.0, 8.0]
-  #   nominal_velocity: 0.0
-  #   target: [15.0, 4.0]
-  #   behavior: loop
-  #   yaw: 1.5708
-  
+agents:
   cone1:
     type: cone
-    position: [50.0, 8.0]
+    position: [3, 10.0]
     nominal_velocity: 0.0
     target: [60.0, 8.0]
     behavior: loop
     yaw: 0
 
   cone2:
     type: cone
-    position: [56.0, 8.0]
+    position: [7, 10.0]
     nominal_velocity: 0.0
     target: [60.0, 8.0]
     behavior: loop
     yaw: 0
 
   cone3:
     type: cone
-    position: [50.0, 4.0]
+    position: [3, 4.0]
     nominal_velocity: 0.0
     target: [60.0, 8.0]
     behavior: loop
     yaw: 0
 
-  # cone4:
-  #   type: cone
-  #   position: [56.0, 4.0]
+  cone4:
+    type: cone
+    position: [7, 4.0]
+    nominal_velocity: 0.0
+    target: [60.0, 8.0]
+    behavior: loop
+    yaw: 0
+
+  # ped1:
+  #   type: medium_truck
+  #   position: [2.25, 8.0]
   #   nominal_velocity: 0.0
-  #   target: [60.0, 8.0]
+  #   target: [20.0, 8.0]
   #   behavior: loop
-  #   yaw: 0
+  #   yaw: 1.5708
+
+  # ped2:
+  #   type: medium_truck
+  #   position: [7.75, 8.0]
+  #   nominal_velocity: 0.0
+  #   target: [15.0, 4.0]
+  #   behavior: loop
+  #   yaw: 1.5708

scenes/parking/demo_3.yaml

@@ -0,0 +1,34 @@
+vehicle_state: [-1.0, -2.0, 0.0, 0.0, 0.0]
+
+agents:
+  cone1:
+    type: cone
+    position: [6, 9.0]
+    nominal_velocity: 0.0
+    target: [60.0, 8.0]
+    behavior: loop
+    yaw: 0
+
+  cone2:
+    type: cone
+    position: [10, 9.0]
+    nominal_velocity: 0.0
+    target: [60.0, 8.0]
+    behavior: loop
+    yaw: 0
+
+  cone3:
+    type: cone
+    position: [3, 4.0]
+    nominal_velocity: 0.0
+    target: [60.0, 8.0]
+    behavior: loop
+    yaw: 0
+
+  cone4:
+    type: cone
+    position: [7, 4.0]
+    nominal_velocity: 0.0
+    target: [60.0, 8.0]
+    behavior: loop
+    yaw: 0

scenes/parking/demo_4.yaml

@@ -0,0 +1,19 @@
+vehicle_state: [0.0, 0.0, 0.0, 0.0, 0.0]
+agents:
+  ped1:
+    type: medium_truck
+    position: [2.5, 8.0]
+    nominal_velocity: 0.0
+    target: [20.0, 8.0]
+    behavior: loop
+    yaw: 1.5708
+
+  ped2:
+    type: medium_truck
+    position: [7.5, 8.0]
+    nominal_velocity: 0.0
+    target: [15.0, 4.0]
+    behavior: loop
+    yaw: 1.5708
+
+