fixes to get stuff working on board

Author: KenC1014

GEMstack/knowledge/defaults/computation_graph.yaml

@@ -57,7 +57,7 @@ components:
       outputs: route
   - route_planning_component: # one way
       inputs: all
-      outputs: trajectory
+      outputs: route
   - motion_planning:
       inputs: all
       outputs: trajectory

GEMstack/onboard/planning/longitudinal_planning.py

@@ -89,7 +89,7 @@ def longitudinal_plan(path, acceleration, deceleration, max_speed, current_speed
         if DEBUG:
             print(f"[DEBUG] Phase 2 - Cruise: distance = {cruise_distance:.2f}")
             print(f"[DEBUG] Phase 3 - Final Decel: distance = {final_decel_distance:.2f}")
-
+            
         times = []
         for s in s_vals:
             if s <= initial_decel_distance:  # Phase 1: Initial deceleration to max_speed

GEMstack/onboard/planning/parking_planning.py

@@ -465,7 +465,7 @@ def vehicle_state_to_first_order(self, vehicle_state: VehicleState) -> Tuple[flo
 
         return (x,y,theta,t)
 
-    def update(self, state : AllState) -> Trajectory:
+    def update(self, state : AllState) -> Route:
         """_summary_
 
         Args:
@@ -511,10 +511,10 @@ def update(self, state : AllState) -> Trajectory:
         print("===========================")
         print(f"Points: {points}")
         print(f"Times: {times}")
-        # route = Path(frame=vehicle.pose.frame, points=points)
+        route = Path(frame=vehicle.pose.frame, points=points)
         # traj = longitudinal_plan(route, 2, -2, 10, vehicle.v, "milestone")
         print(traj)
-        return traj 
+        return route 
 
 
     from rospy.exceptions import ROSInitException

GEMstack/onboard/planning/route_planning_component.py

@@ -26,7 +26,7 @@ def state_inputs(self):
         return ["all"]
 
     def state_outputs(self) -> List[str]:
-        return ['trajectory']
+        return ['route']
 
     def rate(self):
         return 10.0 # very high for our computation ability
@@ -44,12 +44,14 @@ def update(self, state: AllState):
         if state.mission_plan.planner_type.name == "PARKING":
             print("I am in PARKING mode")
             # Not sure where I should construct this object
-            if isinstance(self.planner, StraightLineMotion): # we are transitioning from scanning to parking
+            # if isinstance(self.planner, StraightLineMotion): # we are transitioning from scanning to parking
 
-                # Check if the car is still in motion from scanning behavior
-                # if it is, we need to stop it before parking
-                self.planner.brake() # get car totally stopped before parking
-                state.vehicle.pose.yaw = 0 # needed this to avoid a weird error in the parking planner
+            #     # Check if the car is still in motion from scanning behavior
+            #     # if it is, we need to stop it before parking
+
+            #     # print("I AM BRAKING")
+            # self.planner.brake() # get car totally stopped before parking
+            state.vehicle.pose.yaw = 0 # needed this to avoid a weird error in the parking planner
 
             self.planner = ParkingPlanner()
 
@@ -72,11 +74,11 @@ def update(self, state: AllState):
             # print("Vehicle y:", state.vehicle.pose.y)
             # print("Vehicle yaw:", state.vehicle.pose.yaw)
             desired_points = [(state.vehicle.pose.x, state.vehicle.pose.y),
-                              (state.vehicle.pose.x + 10, state.vehicle.pose.y)]
+                              (state.vehicle.pose.x + 1, state.vehicle.pose.y)]
             desired_path = Path(state.vehicle.pose.frame, desired_points)
 
             # @TODO these are constants we need to get from settings
-            return longitudinal_plan(desired_path,1,1,3,state.vehicle.v)
+            return desired_path
             # self.planner.update_speed()
 
             # We want to just go straight

launch/parking_detection.yaml

@@ -32,11 +32,12 @@ drive: # Full pipeline on board
     planning:
         parking_component:
             type: ParkingSim
-    #     route_planning_component:
-    #         type: RoutePlanningComponent
-    # trajectory_tracking:
-    #         type: pure_pursuit.PurePursuitTrajectoryTracker
-    #         print: False
+        route_planning_component:
+            type: RoutePlanningComponent
+        motion_planning: longitudinal_planning.YieldTrajectoryPlanner
+        trajectory_tracking:
+                type: pure_pursuit.PurePursuitTrajectoryTracker
+                print: False
 #usually can keep this constant
 computation_graph: !include "../GEMstack/knowledge/defaults/computation_graph.yaml"