To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit fa7a4cd2 authored by roangel's avatar roangel
Browse files

added time measurement

parent ed2750e6
......@@ -199,7 +199,7 @@ VectorXtype x0;
std::mutex MPC_mutex;
int N = 1;
int N = 2;
// ROS Publisher for debugging variables
......@@ -495,12 +495,22 @@ DebugMsg debugMsg;
// float stateInertialEstimate[12];
int mean_time_counter = 0;
void do_MPC(int* publish_rate)
{
ros::Rate loop_rate(*publish_rate);
double elapsed_secs;
double array_times[10] = {0};
while(ros::ok())
{
begin_t = ros::Time::now();
// Put the MPC loop here:
// load x0 protected
MPC_mutex.lock();
......@@ -508,7 +518,8 @@ void do_MPC(int* publish_rate)
MPC_mutex.unlock();
// Do the MPC step:
U_0 = mympc_varying_another_ADP(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
// U_0 = mympc_varying_another_ADP(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
VectorUtype u = U_0.tail(4); // this is the input to apply next
// prediction of next N states:
......@@ -547,13 +558,31 @@ void do_MPC(int* publish_rate)
raw_input[3] = (float)u(3);
MPC_mutex.unlock();
elapsed_secs = ros::Time::now().toSec() - begin_t.toSec();
if(mean_time_counter > 10)
{
mean_time_counter = 0;
double sum = 0;
for(int i = 0; i < 9; i++)
{
array_times[i] = array_times[i+1];
sum += array_times[i];
}
array_times[9] = elapsed_secs;
sum += array_times[9];
std::cout << "++++++++++++++++++solver iteration time: " << sum/10 << std::endl;
}
// end MPC loop
loop_rate.sleep();
double elapsed_secs = ros::Time::now().toSec() - begin_t.toSec();
std::cout << "elapsed secs: " << elapsed_secs << std::endl;
begin_t = ros::Time::now();
elapsed_secs = ros::Time::now().toSec() - begin_t.toSec();
std::cout << " 50 Hz loop secs: " << elapsed_secs << std::endl;
mean_time_counter++;
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment