update

Author: derekzyn

GEMstack/onboard/planning/leave_parking_trajectory_planning.py

@@ -54,7 +54,7 @@ def update(self, state: AllState):
         vehicle = state.vehicle
         roadgraph = state.roadgraph
 
-        print('Input:')
+        print('Input states:')
         print(intent)
         print(mission)
 
@@ -74,7 +74,7 @@ def update(self, state: AllState):
         intent.intent = VehicleIntentEnum.DRIVING
         mission.type = MissionEnum.DRIVE
 
-        print('Output:')
+        print('Output states:')
         print(intent)
         print(mission)
 

GEMstack/onboard/planning/longitudinal_planning.py

@@ -303,7 +303,7 @@ def update(self, state: AllState):
         mission = state.mission
         t = state.t
 
-        print('Input:')
+        print('Input states:')
         print(mission)
 
         traj = None
@@ -405,7 +405,9 @@ def update(self, state: AllState):
             zero_path = [[0, 0, 0]]
             traj = Trajectory(frame=frame, points=zero_path, times=[0])
 
-        print('Output:')
+        print('Output states:')
         print(mission)
+        print('Trajectory:')
+        print(traj)
 
         return traj, mission

GEMstack/onboard/planning/mission_planning.py

@@ -8,6 +8,9 @@
 
 
 class SummoningMissionPlanner(Component):
+    def __init__(self):
+        self.count = 0
+
     def state_inputs(self):
         return ['all']
 
@@ -18,16 +21,20 @@ def rate(self):
         return 0.5
 
     def update(self, state: AllState):
+        self.count += 1
+        print('Counting:', self.count)
+
         task = state.task
         mission = state.mission
 
-        print('Input:')
+        print('Input states:')
         print(task)
         print(mission)
 
         # TODO: Modify to a GET request to get task phase from the server
         if task.phase == TaskEnum.IDLE:
-            task.phase = TaskEnum.START
+            if self.count == 3:
+                task.phase = TaskEnum.START
 
         elif mission.type == MissionEnum.IDLE and task.phase == TaskEnum.START:
             mission.type = MissionEnum.PLAN
@@ -40,7 +47,7 @@ def update(self, state: AllState):
 
         # TODO: POST update status to the server
 
-        print('Output:')
+        print('Output states:')
         print(task)
         print(mission)
 

GEMstack/onboard/planning/parking_planning.py

@@ -34,7 +34,7 @@ def update(self, state: AllState):
         intent = state.intent
         roadgraph = state.roadgraph
 
-        print('Input:')
+        print('Input states:')
         print(intent)
         print(mission)
 
@@ -44,7 +44,7 @@ def update(self, state: AllState):
         elif mission.type == MissionEnum.UNPARK:
             intent = leave_parking_plan(roadgraph)
 
-        print('Output:')
+        print('Output states:')
         print(intent)
 
         return intent

GEMstack/onboard/planning/parking_trajectory_planning.py

@@ -55,7 +55,7 @@ def update(self, state: AllState):
         vehicle = state.vehicle
         roadgraph = state.roadgraph
 
-        print('Input:')
+        print('Input states:')
         print(intent)
         print(mission)
 
@@ -75,7 +75,7 @@ def update(self, state: AllState):
         intent.intent = VehicleIntentEnum.IDLE
         mission.type = MissionEnum.IDLE
 
-        print('Output:')
+        print('Output states:')
         print(intent)
         print(mission)
 

GEMstack/onboard/planning/route_planning.py

@@ -155,21 +155,25 @@ def update(self, state: AllState):
         route = state.route
         mission = state.mission
 
-        print('Input:')
+        print('Input states:')
         print(mission)
 
         if mission.type == MissionEnum.PLAN:
             mission.type = MissionEnum.UNPARK
             print('Output:')
             print(mission)
-            return self.route, mission
-        elif route:
-            pass
-        # else:   # avoid error
-        #     route = Route(frame=ObjectFrameEnum.START,points=[[0, 0],[0.1, 0], [0.2, 0]])
+            route = self.route
 
-        print('Output:')
+        elif route is None:
+            route = Route(frame=ObjectFrameEnum.START, points=[[0, 0]])
+
+        # route = self.route
+
+        print('Output states:')
         print(mission)
+        print('Route:')
+        print(route)
+
         return route, mission
 
 
@@ -202,7 +206,7 @@ def update(self, state: AllState):
         mission = state.mission
         current_pose = state.vehicle.pose
 
-        print('Input:')
+        print('Input states:')
         print(mission)
 
         # TODO: get from the server
@@ -241,14 +245,10 @@ def update(self, state: AllState):
             else:
                 raise ValueError("Unknown frame argument")
 
-        elif route:
-            pass
-
-        else:
-            route = Route(frame=ObjectFrameEnum.START,points=[[0, 0],[1, 0], [2, 0]])
-            return route, mission
+        elif route is None:
+            route = Route(frame=ObjectFrameEnum.START,points=[[0, 0]])
 
-        print('Output:')
+        print('Output states:')
         print(mission)
 
         return route, mission