renaming parking goal for clarity + adding on board launch functionality

Author: Aadarsh

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -23,7 +23,7 @@ components:
       outputs: vehicle_lane
   - parking_detection:
       inputs: obstacles
-      outputs: [goal, obstacles]
+      outputs: [parking_goal, obstacles]
   - sign_detection: 
       inputs: [vehicle, roadgraph]
       outputs: roadgraph.signs

GEMstack/onboard/perception/parking_detection.py

@@ -76,7 +76,7 @@ def state_inputs(self) -> list:
         return ['obstacles']
 
     def state_outputs(self) -> list:
-        return ['goal', 'obstacles']
+        return ['parking_goal', 'obstacles']
 
 
     def visualize(self, 

GEMstack/onboard/planning/parking_component.py

@@ -24,14 +24,10 @@ def update(self, state: AllState):
         # Calculate elapsed time since initialization.
         elapsed_time = time.time() - self.start_time
 
-        # Reading goal from state
-        # print(f"\n AllState (parking goal): {state.goal} \n")
-        # print(f"AllState (parking obstacles): {state.obstacles} \n")
-
         # After a goal is detected, change the mission plan to use PARKING.
-        if state.goal:
+        if state.parking_goal:
             print("\n Parking goal available. Entering PARKING mode......")
-            mission_plan = MissionObjective(PlannerEnum.PARKING, state.goal)
+            mission_plan = MissionObjective(PlannerEnum.PARKING, state.parking_goal)
         else:
             print("\n Entering SCANNING mode......")
             mission_plan = MissionObjective(PlannerEnum.SCANNING)

GEMstack/onboard/planning/parking_route_planner.py

@@ -36,12 +36,12 @@ def __init__(self, vehicle=None, obstacles=None, actions=None):
         self._vehicle = None
 
         # settings
-        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.T = settings.get("run.drive.planning._route_planner.dubins.integrator.time_step", 1.5)
+        self.dt = settings.get("run.drive.planning._route_planner.dubins.integrator.dt", .25)
+        self.max_v = settings.get("run.drive.planning._route_planner.dubins.actions.max_velocity", 0.75)
+        self.max_turn_rate = settings.get("run.drive.planning._route_planner.dubins.actions.max_turn_rate", 0.3)
+        self.tolerance = settings.get("run.drive.planning._route_planner.dubins.tolerance", 0.5)
+        self.heuristic_string = settings.get("run.drive.planning._route_planner.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)
@@ -255,7 +255,7 @@ def __init__(self):
         # self.planner = ParkingSolverSecondOrderDubins()
         self.planner = ParkingSolverFirstOrderDubins()
 
-        self.iterations = settings.get("run.drive.planning.route_planning_component.astar.iterations", 200)
+        self.iterations = settings.get("run.drive.planning._route_planner.astar.iterations", 200)
 
     def state_inputs(self):
         return ['all']
@@ -335,7 +335,7 @@ def update(self, state : AllState) -> Route:
         all_obstacles = {**agents, **obstacles}
         # print(f"Obstacles {obstacles}")
         print(f"Agents {agents}")
-        goal_pose = state.goal
+        goal_pose = state.parking_goal
         assert goal_pose.frame == ObjectFrameEnum.CURRENT
         print("Checking VALUE: ", state.start_vehicle_pose)
         print("Checking VALUE: ", state.vehicle)

GEMstack/onboard/planning/route_planning_component.py

@@ -35,7 +35,7 @@ def rate(self):
     def update(self, state: AllState):
 
         if state.mission_plan.type.name == "PARKING" and not self.compute_parking_route:
-            print("I am in BRAKING mode")
+            # print("I am in BRAKING mode")
 
             desired_points = [(state.vehicle.pose.x, state.vehicle.pose.y),
                               (state.vehicle.pose.x, state.vehicle.pose.y)]
@@ -48,7 +48,7 @@ def update(self, state: AllState):
             return desired_path
 
         elif state.mission_plan.type.name == "PARKING" and self.compute_parking_route and not self.done_computing:
-            print("I am in PARKING mode")
+            # print("I am in PARKING mode")
             state.vehicle.pose.yaw = 0 # needed this to avoid a weird error in the parking planner
 
             if not self.already_computed:
@@ -58,11 +58,11 @@ def update(self, state: AllState):
                 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
-
+                self.route.points = self.route.points[:-2] # temporary fix to avoid going too far into the parking space, next perception PR will resolve
             return self.route
 
         elif state.mission_plan.type.name == "SCANNING":
-            print("I am in SCANNING mode")
+            # 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.START, desired_points)

GEMstack/state/all.py

@@ -24,7 +24,7 @@ class AllState(SceneState):
     agent_intents : Dict[str,AgentIntentMixture] = field(default_factory=dict)
     relations : List[EntityRelation] = field(default_factory=list)
     predicates : PredicateValues = field(default_factory=PredicateValues)
-    goal: ObjectPose = None
+    parking_goal: ObjectPose = None
 
     # planner-output state
     mission : MissionObjective = field(default_factory=MissionObjective)