add debug info

src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp

@@ -1500,6 +1500,10 @@ void NMPCgenerator::updateCostFunction()
   // p_xy_Y  =   0.5 * a * Pvu^T   * ( Pvs * c_k_y - dY^ref )
   //           + 0.5 * b * Pzu^T   * ( Pzs * c_k_y - v_kp1 * f_k_y )
   // p_xy_Fy = - 0.5 * b * V_kp1^T * ( Pzs * c_k_y - v_kp1 * f_k_y )
+#ifdef DEBUG
+  cout << vel_ref_.Global.X << " "
+       << vel_ref_.Global.Y << endl;
+#endif
   p_xy_X_ =    alpha_ * MAL_RET_A_by_B(MAL_RET_TRANSPOSE(Pvu_  ), Pvsc_x_ - vel_ref_.Global.X_vec)
              + beta_  * MAL_RET_A_by_B(MAL_RET_TRANSPOSE(Pzu_  ), Pzsc_x_ - v_kp1f_x_                 );
 #ifdef DEBUG