Commit e12538ef authored by Alexander Ryndin's avatar Alexander Ryndin
Browse files

add benchmark for motion planning

parent 0f1777bf
#ifndef BENCHMARK
#define BENCHMARK
#include "./model/Helper.h"
#include "./model/de_rwth_armin_modeling_autopilot_motion_motionPlanning.h"
#include <iostream>
#include <chrono>
// https://stackoverflow.com/a/19471595/1432357
class Timer
{
public:
Timer() : beg_(clock_::now()) {}
void reset() { beg_ = clock_::now(); }
double elapsed() const {
return std::chrono::duration_cast<second_>
(clock_::now() - beg_).count(); }
private:
typedef std::chrono::high_resolution_clock clock_;
typedef std::chrono::duration<double, std::ratio<1> > second_;
std::chrono::time_point<clock_> beg_;
};
int main () {
std::cout << "initialization...\n";
Helper::init();
de_rwth_armin_modeling_autopilot_motion_motionPlanning motionPlanning;
motionPlanning.init();
const int N = 1000000;
std::cout << "running benchmark\n";
Timer tmr;
for (int i=0; i<N; i++) {
motionPlanning.currentDirectionX = 0.0;
motionPlanning.currentDirectionY = 1.0;
motionPlanning.desiredDirectionX = 1.0;
motionPlanning.desiredDirectionY = 1.0;
motionPlanning.signedDistanceToTrajectory = 0.15;
motionPlanning.currentVelocity = 10.0;
motionPlanning.desiredVelocity = 11.0;
motionPlanning.execute();
}
double t = tmr.elapsed();
double avgDuration = t / N;
std::cout << "avgDuration = " << avgDuration << " sec\n";
return 0;
}
#endif
rem call variables.bat
set OUTPUT_DIR=build
if exist "%OUTPUT_DIR%" rmdir "%OUTPUT_DIR%" /s /q
mkdir "%OUTPUT_DIR%"
g++ -I"%PATH_TO_JDK_HOME%\include" ^
-I"%PATH_TO_JDK_HOME%\include\win32" ^
-I"%PATH_TO_OCTAVE_HOME%\include\octave-4.2.1" ^
-I"%PATH_TO_OCTAVE_HOME%\include\octave-4.2.1\octave" ^
-L"%PATH_TO_OCTAVE_HOME%\bin" ^
-o "%OUTPUT_DIR%\benchmark.exe" ^
benchmark.cpp ^
-loctave-4 -loctinterp-4
#ifndef HELPER_H
#define HELPER_H
#define _GLIBCXX_USE_CXX11_ABI 0
#include <iostream>
#include <octave/oct.h>
#include <octave/octave.h>
#include <octave/parse.h>
#include <octave/interpreter.h>
#include <stdarg.h>
#include <initializer_list>
class Helper
{
public:
static void init()
{
string_vector argv(2);
argv(0) = "embedded";
argv(1) = "-q";
octave_main(2, argv.c_str_vec(), 1);
//octave_debug=1;
//feval ("pkg", ovl ("load", "all"), 0);
}
static octave_value_list convertToOctaveValueList(double a)
{
octave_value_list in;
in(0) = a;
return in;
}
static octave_value_list convertToOctaveValueList(Matrix a)
{
octave_value_list in;
in(0) = a;
return in;
}
static octave_value_list convertToOctaveValueList(RowVector a)
{
octave_value_list in;
in(0) = a;
return in;
}
static octave_value_list convertToOctaveValueList(ColumnVector a)
{
octave_value_list in;
in(0) = a;
return in;
}
static octave_value_list convertToOctaveValueList(double a, double b)
{
octave_value_list in;
in(0) = a;
in(1) = b;
return in;
}
static octave_value_list convertToOctaveValueList(std::initializer_list<double> args)
{
octave_value_list in;
int counter = 0;
for(double element : args) {
in(counter) = octave_value(element);
++counter;
}
return in;
}
static octave_value_list convertToOctaveValueList(Matrix a, double b)
{
octave_value_list in;
in(0) = a;
in(1) = b;
return in;
}
static octave_value_list convertToOctaveValueList(RowVector a, double b)
{
octave_value_list in;
in(0) = a;
in(1) = b;
return in;
}
static octave_value_list convertToOctaveValueList(ColumnVector a, double b)
{
octave_value_list in;
in(0) = a;
in(1) = b;
return in;
}
static octave_value_list callOctaveFunction(octave_value_list in, std::string functionName,int argsOut)
{
/*octave_idx_type n = 2;
octave_value_list in;
for(octave_idx_type i = 0; i < n; i++)
in(i) = octave_value(5 * (i + 2));
octave_value_list out = feval("gcd", in, 1);
if(!error_state && out.length() > 0)
std::cout << "GCD of [" << in(0).int_value() << ", " << in(1).int_value() << "] is " << out(0).int_value()
<< std::endl;
else
std::cout << "invalid\n";
clean_up_and_exit(0);*/
/* if(functionName=="eigs")
return feval(functionName, in, 2);
else if(functionName=="kmeans")
return feval(functionName, in, 2);
*/
return feval(functionName, in, argsOut);
}
static int callOctaveFunctionIntFirstResult(octave_value_list in, std::string functionName, int argsOut)
{
// printf("callOctaveFunctionIntFirstResult pre return functionName: %s\n",functionName.c_str());
return callOctaveFunction(in, functionName,argsOut)(0).int_value();
}
static double callOctaveFunctionDoubleFirstResult(octave_value_list in, std::string functionName, int argsOut)
{
// printf("callOctaveFunctionDoubleFirstResult pre return functionName: %s\n",functionName.c_str());
return callOctaveFunction(in, functionName,argsOut)(0).double_value();
}
static Matrix callOctaveFunctionMatrixFirstResult(octave_value_list in, std::string functionName, int argsOut)
{
return callOctaveFunction(in, functionName,argsOut)(0).matrix_value();
}
static ColumnVector callOctaveFunctionColumnVectorFirstResult(octave_value_list in, std::string functionName, int argsOut)
{
printf("pre Call %s\n", functionName.c_str());
try {
in=octave_value_list();
octave_value_list list = callOctaveFunction(in, functionName,argsOut);
printf("post Call %s\n", functionName.c_str());
return list(0).array_value().as_column();
} catch(const std::exception& e) {
printf("%s\n", e.what());
}
return ColumnVector();
}
static RowVector callOctaveFunctionRowVectorFirstResult(octave_value_list in, std::string functionName, int argsOut)
{
return callOctaveFunction(in, functionName,argsOut)(0).array_value().as_row();
}
static int callOctaveFunctionIntSecondResult(octave_value_list in, std::string functionName, int argsOut)
{
return callOctaveFunction(in, functionName,argsOut)(1).int_value();
}
static double callOctaveFunctionDoubleSecondResult(octave_value_list in, std::string functionName, int argsOut)
{
return callOctaveFunction(in, functionName,argsOut)(1).double_value();
}
static Matrix callOctaveFunctionMatrixSecondResult(octave_value_list in, std::string functionName, int argsOut)
{
return callOctaveFunction(in, functionName,argsOut)(1).matrix_value();
}
static ColumnVector callOctaveFunctionColumnVectorSecondResult(octave_value_list in, std::string functionName, int argsOut)
{
return callOctaveFunction(in, functionName,argsOut)(1).array_value().as_column();
}
static RowVector callOctaveFunctionRowVectorSecondResult(octave_value_list in, std::string functionName, int argsOut)
{
return callOctaveFunction(in, functionName,argsOut)(1).array_value().as_row();
}
static Matrix getMatrixFromOctaveListFirstResult(octave_value_list list){
return list(0).matrix_value();
}
static RowVector getRowVectorFromOctaveListFirstResult(octave_value_list list){
return list(0).array_value().as_row();
}
static ColumnVector getColumnVectorFromOctaveListFirstResult(octave_value_list list){
return list(0).array_value().as_column();
}
static double getDoubleFromOctaveListFirstResult(octave_value_list list){
return list(0).double_value();
}
static int getIntFromOctaveListFirstResult(octave_value_list list){
return list(0).int_value();
}
};
#endif // HELPER_H
\ No newline at end of file
#ifndef DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING
#define DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "octave/oct.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_keepDirection.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_sac.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_sum1.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsSteering.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsBrakes.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsEngine.h"
class de_rwth_armin_modeling_autopilot_motion_motionPlanning{
public:
double currentDirectionX;
double currentDirectionY;
double desiredDirectionX;
double desiredDirectionY;
double signedDistanceToTrajectory;
double currentVelocity;
double desiredVelocity;
double engine;
double steering;
double brakes;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_keepDirection keepDirection;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_sac sac;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_sum1 sum1;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes engineAndBrakes;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsSteering boundsSteering;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsBrakes boundsBrakes;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsEngine boundsEngine;
void init()
{
keepDirection.init();
sac.init();
sum1.init();
engineAndBrakes.init();
boundsSteering.init();
boundsBrakes.init();
boundsEngine.init();
}
void execute()
{
sac.signedDistanceToTrajectory = signedDistanceToTrajectory;
sac.execute();
sum1.t1 = keepDirection.steeringAngle;
sum1.t2 = sac.steeringAngleCorrection;
sum1.execute();
keepDirection.currentDirectionX = currentDirectionX;
keepDirection.currentDirectionY = currentDirectionY;
keepDirection.desiredDirectionX = desiredDirectionX;
keepDirection.desiredDirectionY = desiredDirectionY;
keepDirection.execute();
engineAndBrakes.currentVelocity = currentVelocity;
engineAndBrakes.desiredVelocity = desiredVelocity;
engineAndBrakes.execute();
boundsSteering.input = sum1.result;
boundsSteering.execute();
boundsBrakes.input = engineAndBrakes.brakes;
boundsBrakes.execute();
boundsEngine.input = engineAndBrakes.engine;
boundsEngine.execute();
engine = boundsEngine.output;
steering = boundsSteering.output;
brakes = boundsBrakes.output;
}
};
#endif
#ifndef DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSBRAKES
#define DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSBRAKES
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "octave/oct.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsBrakes_eb.h"
class de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsBrakes{
public:
double input;
double output;
double CONSTANTPORT3;
double CONSTANTPORT4;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsBrakes_eb eb;
void init()
{
this->CONSTANTPORT3 = 0;
this->CONSTANTPORT4 = 3;
eb.init();
}
void execute()
{
eb.lowerBound = CONSTANTPORT3;
eb.upperBound = CONSTANTPORT4;
eb.input = input;
eb.execute();
output = eb.output;
}
};
#endif
#ifndef DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSBRAKES_EB
#define DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSBRAKES_EB
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "octave/oct.h"
class de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsBrakes_eb{
public:
double lowerBound;
double upperBound;
double input;
double output;
void init()
{
}
void execute()
{
if((input < lowerBound)){
output = lowerBound;
}
else if((input > upperBound)){
output = upperBound;
}
else {
output = input;
}
}
};
#endif
#ifndef DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSENGINE
#define DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSENGINE
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "octave/oct.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsEngine_eb.h"
class de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsEngine{
public:
double input;
double output;
double CONSTANTPORT5;
double CONSTANTPORT6;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsEngine_eb eb;
void init()
{
this->CONSTANTPORT5 = 0;
this->CONSTANTPORT6 = 2.5;
eb.init();
}
void execute()
{
eb.lowerBound = CONSTANTPORT5;
eb.upperBound = CONSTANTPORT6;
eb.input = input;
eb.execute();
output = eb.output;
}
};
#endif
#ifndef DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSENGINE_EB
#define DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSENGINE_EB
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "octave/oct.h"
class de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsEngine_eb{
public:
double lowerBound;
double upperBound;
double input;
double output;
void init()
{
}
void execute()
{
if((input < lowerBound)){
output = lowerBound;
}
else if((input > upperBound)){
output = upperBound;
}
else {
output = input;
}
}
};
#endif
#ifndef DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSSTEERING
#define DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSSTEERING
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "octave/oct.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsSteering_eb.h"
class de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsSteering{
public:
double input;
double output;
double CONSTANTPORT1;
double CONSTANTPORT2;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsSteering_eb eb;
void init()
{
this->CONSTANTPORT1 = -0.785;
this->CONSTANTPORT2 = 0.785;
eb.init();
}
void execute()
{
eb.lowerBound = CONSTANTPORT1;
eb.upperBound = CONSTANTPORT2;
eb.input = input;
eb.execute();
output = eb.output;
}
};
#endif
#ifndef DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSSTEERING_EB
#define DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_BOUNDSSTEERING_EB
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "octave/oct.h"
class de_rwth_armin_modeling_autopilot_motion_motionPlanning_boundsSteering_eb{
public:
double lowerBound;
double upperBound;
double input;
double output;
void init()
{
}
void execute()
{
if((input < lowerBound)){
output = lowerBound;
}
else if((input > upperBound)){
output = upperBound;
}
else {
output = input;
}
}
};
#endif
#ifndef DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_ENGINEANDBRAKES
#define DE_RWTH_ARMIN_MODELING_AUTOPILOT_MOTION_MOTIONPLANNING_ENGINEANDBRAKES
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "octave/oct.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes_pidParams.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes_pidError.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes_abs1.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes_pid.h"
#include "de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes_engineOrBrakes.h"
class de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes{
public:
double currentVelocity;
double desiredVelocity;
double engine;
double brakes;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes_pidParams pidParams;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes_pidError pidError;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes_abs1 abs1;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes_pid pid;
de_rwth_armin_modeling_autopilot_motion_motionPlanning_engineAndBrakes_engineOrBrakes engineOrBrakes;
void init()
{
pidParams.init();
pidError.init();
abs1.init();
pid.init();
engineOrBrakes.init();
}
void execute()
{
pidParams.currentVelocity = currentVelocity;
pidParams.desiredVelocity = desiredVelocity;
pidParams.execute();
pidError.currentVelocity = currentVelocity;
pidError.desiredVelocity = desiredVelocity;
pidError.execute();
abs1.input = pidError.error;
abs1.execute();
pid.paramP = pidParams.paramP;
pid.paramI = pidParams.paramI;
pid.paramD = pidParams.paramD;
pid.paramDecayCoefficient = pidParams.paramDecayCoefficient;
pid.error = abs1.output;
pid.execute();
engineOrBrakes.error = pidError.error;