Fix trajectory sending in planner.py

LITdrive · Oct 26, 2018 · 02205f4 · 02205f4
1 parent a446040
commit 02205f4
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 24 deletions.
diff --git a/src/aadcUserPython/litdrive/litdrive/selfdriving/__main__.py b/src/aadcUserPython/litdrive/litdrive/selfdriving/__main__.py
@@ -101,7 +101,6 @@ def _process(car: Car, commander: Commander,
                  lz1, lz2, lz3, lz4, lz5, lz6, lz7, lz8, lz9, lz10,
                  lz11, lz12, lz13, lz14, lz15, lz16, lz17, lz18, lz19, lz20):
         # print("- sensor")
-        print(position)
         # debug output
         # print(json.dumps([position, measured_speed, signs, lidar, ultrasonic, imu,
         #                  controller_leverage, controller_feedback, siren, lidar_break], indent=2))
@@ -140,9 +139,9 @@ def _process(car: Car, commander: Commander,
                                               float(leverage_p))
 
         print("SPEED: " + str(commander.out_speed))
-        print(out_trajectories)
         print(car.planner)
-        return (0, 0 if break_signal else commander.out_speed), out_trajectories
+        print(position)
+        return (0, 0 if break_signal else commander.out_speed), out_trajectories, None, None, None, None, None
 
 
 if __name__ == "__main__":
@@ -153,5 +152,5 @@ def _process(car: Car, commander: Commander,
         "maneuverListFile":
             abspath(join(dirname(__file__), r'../../../../../configuration_files/jury/maneuver.xml')),
         "pickledOpenDriveMap":
-            abspath(join(dirname(__file__), r'../../../../../configuration_files/maps/qualifying_2018_litd.pickle'))
+            abspath(join(dirname(__file__), r'../../../../../configuration_files/maps/scp3_2018_litd.pickle'))
     })
diff --git a/src/aadcUserPython/litdrive/litdrive/selfdriving/planning/planner.py b/src/aadcUserPython/litdrive/litdrive/selfdriving/planning/planner.py
@@ -58,24 +58,6 @@ def addManeuver(self, maneuver:ManeuverState):
         if(len(self.decisions_to_submit)>0):
             self.state=PlannerState.FORWARD_NORMAL
 
-    @staticmethod
-    def build_trajectory_array_buffer(trajectories):
-        if not trajectories or len(trajectories) == 0:
-            return None
-
-        # assert len(trajectories) <= TRAJECTORY_ARRAY_SIZE
-        print(trajectories)
-        trajectories = trajectories[0:10]
-        buffer = ([[0] * TRAJECTORY_NUM_FIELDS] * TRAJECTORY_ARRAY_SIZE)
-
-        for i, trajectory in enumerate(trajectories):
-            buffer[i] = trajectory
-
-        def _flatten(data):
-            return [item for sublist in data for item in sublist]
-
-        return [len(trajectories)] + _flatten(buffer)
-
     def update(self, controller_done_id, controller_current_id, controller_current_p):
         if(self.state<=PlannerState.INVALID ):
             print("ERROR: Planner is in INVALID-State!!!")
@@ -131,11 +113,16 @@ def update(self, controller_done_id, controller_current_id, controller_current_p
                 buffer = [0] * TRAJECTORY_NUM_FIELDS * TRAJECTORY_ARRAY_SIZE
                 for i in range(0, len(ids)):
                     x_poly, y_poly = lanes[i].getPolys(False)
-                    buffer[i*TRAJECTORY_NUM_FIELDS:(i+1)*TRAJECTORY_NUM_FIELDS]=[controller_ids[i], *tuple(x_poly)[::-1], *tuple(y_poly)[::-1], 0.0, 1.0, False]
+
+                    # presenting the ugliest zero-pad in history ...
+                    x_poly_pad, y_poly_pad = np.zeros(4), np.zeros(4)
+                    x_poly_pad[0:x_poly.order+1]= np.flip(x_poly, 0)
+                    y_poly_pad[0:y_poly.order+1]= np.flip(y_poly, 0)
+
+                    buffer[(i*TRAJECTORY_NUM_FIELDS):((i+1)*TRAJECTORY_NUM_FIELDS)]=[controller_ids[i], *tuple(x_poly_pad), *tuple(y_poly_pad), 0.0, 1.0, False]
 
                 buffer.insert(0, len(ids))
                 return buffer
-                #return Planner.build_trajectory_array_buffer(poly_buffer)
 
         return None