From 48020e686eacba1dea76d14ff9e807594f7b68e4 Mon Sep 17 00:00:00 2001
From: Willem Melching <willem.melching@gmail.com>
Date: Wed, 14 Jul 2021 19:07:58 +0200
Subject: [PATCH] make sure tidx+1 doesn't exceed TRAJECTORY_SIZE-1 (#21595)

* make sure tidx+1 doesn't exceed TRAJECTORY_SIZE-1

* Update driving.cc

* Update driving.cc

Co-authored-by: deanlee <deanlee3@gmail.com>
Co-authored-by: HaraldSchafer <harald.the.engineer@gmail.com>
---
 selfdrive/modeld/models/driving.cc | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
index 8bb46ee0a3..f7a72b34e1 100644
--- a/selfdrive/modeld/models/driving.cc
+++ b/selfdrive/modeld/models/driving.cc
@@ -253,17 +253,18 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
   // plan
   const float *best_plan = get_plan_data(net_outputs.plan);
   float plan_t_arr[TRAJECTORY_SIZE];
+  std::fill_n(plan_t_arr, TRAJECTORY_SIZE, NAN);
   plan_t_arr[0] = 0.0;
-  int xidx = 1, tidx = 0;
-  for (; xidx<TRAJECTORY_SIZE; xidx++) {
+  for (int xidx=1, tidx=0; xidx<TRAJECTORY_SIZE; xidx++) {
     // increment tidx until we find an element that's further away than the current xidx
-    for (; tidx < TRAJECTORY_SIZE - 1 && best_plan[(tidx+1)*PLAN_MHP_COLUMNS] < X_IDXS[xidx]; tidx++) {}
+    while (tidx < TRAJECTORY_SIZE-1 && best_plan[(tidx+1)*PLAN_MHP_COLUMNS] < X_IDXS[xidx]) {
+      tidx++;
+    }
     float current_x_val = best_plan[tidx*PLAN_MHP_COLUMNS];
     float next_x_val = best_plan[(tidx+1)*PLAN_MHP_COLUMNS];
     if (next_x_val < X_IDXS[xidx]) {
-      // if the plan doesn't extend far enough, set plan_t to the max value (10s), then break and fill the rest with nans
+      // if the plan doesn't extend far enough, set plan_t to the max value (10s), then break
       plan_t_arr[xidx] = T_IDXS[TRAJECTORY_SIZE-1];
-      xidx++;
       break;
     } else {
       // otherwise, interpolate to find `t` for the current xidx
@@ -271,9 +272,6 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
       plan_t_arr[xidx] = p * T_IDXS[tidx+1] + (1 - p) * T_IDXS[tidx];
     }
   }
-  for (; xidx<TRAJECTORY_SIZE; xidx++) {
-    plan_t_arr[xidx] = NAN;
-  }
 
   fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr, true);
   fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr, false);