Aufgrund einer Wartung wird GitLab am 28.09. zwischen 10:00 und 11:00 Uhr kurzzeitig nicht zur Verfügung stehen. / Due to maintenance, GitLab will be temporarily unavailable on 28.09. between 10:00 and 11:00 am.

Tests: Added result/ba to compare with test result

parent 29ba4fd2
cmake_minimum_required(VERSION 3.5)
project (default)
set (CMAKE_CXX_STANDARD 11)
add_subdirectory(rosMsg)
add_subdirectory(ba_system_velocityController_2_)
add_subdirectory(ba_system_stopCommQuality_2_)
add_subdirectory(ba_system_stopCommQuality_1_)
add_subdirectory(ba_system_velocityController_1_)
add_subdirectory(ba_system_collisionDetection)
add_subdirectory(ba_system_intersectionController)
cmake_minimum_required(VERSION 3.5)
project (default)
set (CMAKE_CXX_STANDARD 11)
add_subdirectory(coordinator/)
add_subdirectory(cpp)
add_subdirectory(roscpp)
cmake_minimum_required(VERSION 3.5)
project (Coordinator_ba_system_collisionDetection CXX)
set (CMAKE_CXX_STANDARD 11)
set (THREADS_PREFER_PTHREAD_FLAG ON)
find_package(Threads REQUIRED)
add_executable(Coordinator_ba_system_collisionDetection Coordinator_ba_system_collisionDetection.cpp)
set_target_properties(Coordinator_ba_system_collisionDetection PROPERTIES LINKER_LANGUAGE CXX)
target_link_libraries(Coordinator_ba_system_collisionDetection RosAdapter_ba_system_collisionDetection ba_system_collisionDetection Threads::Threads)
target_include_directories(Coordinator_ba_system_collisionDetection PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
export(TARGETS Coordinator_ba_system_collisionDetection FILE Coordinator_ba_system_collisionDetection.cmake)
\ No newline at end of file
#include <iostream>
#include <thread>
#include <chrono>
#include <atomic>
#include <list>
#include "IAdapter_ba_system_collisionDetection.h"
#include "RosAdapter_ba_system_collisionDetection.h"
#include "ba_system_collisionDetection.h"
using namespace std;
using namespace chrono;
static int exeMs = 100;
bool parseCommandLineParameters(int argc, char* argv[]){
if(argc == 1){
return true;
}
if(argc == 3 && string(argv[1]) == "-t"){
try{
int tmp = stoi(argv[2]);
if(tmp >= 0){
exeMs = tmp;
return true;
}
}catch(...){
//Ignore
}
}
cout << "Usage: " << argv[0] << " [-h | -t sleepTimeMs]\n";
return false;
}
void startMiddleware(IAdapter_ba_system_collisionDetection& adapter,ba_system_collisionDetection& comp,atomic<bool>& done){
adapter.init(&comp);
done = true;
}
int main(int argc, char* argv[])
{
if(!parseCommandLineParameters(argc,argv)){
return 1;
}
atomic<bool> done(false);
ba_system_collisionDetection comp;
comp.init();
list<IAdapter_ba_system_collisionDetection*> adapters;
adapters.push_back(new RosAdapter_ba_system_collisionDetection());
list<thread*> threads;
for(auto a : adapters){
threads.push_back(new thread(startMiddleware,ref(*a),ref(comp),ref(done)));
}
cout << "waiting for all middleware to start\n";
this_thread::sleep_for(seconds(3));
cout << "started! Executing every " << exeMs << "ms\n";
time_point<system_clock> start, end;
while(!done){
start = system_clock::now();
comp.execute();
for(auto a : adapters){
(*a).tick();
}
end = system_clock::now();
int elapsedMs = duration_cast<milliseconds>(end-start).count();
int newSleep = exeMs - elapsedMs;
if(newSleep <= 0){
cout << "Cant keep up! "<< (-newSleep) <<"ms late!\n";
}else{
this_thread::sleep_for(milliseconds(newSleep));
}
}
return 0;
}
\ No newline at end of file
#pragma once
#include "ba_system_collisionDetection.h"
class IAdapter_ba_system_collisionDetection{
public:
virtual ~IAdapter_ba_system_collisionDetection(){}
virtual void init(ba_system_collisionDetection* comp) = 0;
virtual void tick() = 0;
};
cmake_minimum_required(VERSION 3.5)
project(ba_system_collisionDetection LANGUAGES CXX)
add_library(ba_system_collisionDetection ba_system_collisionDetection.h)
target_include_directories(ba_system_collisionDetection PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
set_target_properties(ba_system_collisionDetection PROPERTIES LINKER_LANGUAGE CXX)
export(TARGETS ba_system_collisionDetection FILE ba_system_collisionDetection.cmake)
#ifndef HELPERA_H
#define HELPERA_H
#define _GLIBCXX_USE_CXX11_ABI 0
#include <iostream>
#include "armadillo.h"
#include <stdarg.h>
#include <initializer_list>
using namespace arma;
class HelperA{
public:
static mat getEigenVectors(mat A){
vec eigenValues;
mat eigenVectors;
eig_sym(eigenValues,eigenVectors,A);
return eigenVectors;
}
static vec getEigenValues(mat A){
vec eigenValues;
mat eigenVectors;
eig_sym(eigenValues,eigenVectors,A);
return eigenValues;
}
static mat getKMeansClusters(mat A, int k){
mat clusters;
kmeans(clusters,A.t(),k,random_subset,20,true);
printf("cluster centroid calculation done\n");
std::ofstream myfile;
myfile.open("data after cluster.txt");
myfile << A;
myfile.close();
std::ofstream myfile2;
myfile2.open("cluster centroids.txt");
myfile2 << clusters;
myfile2.close();
mat indexedData=getKMeansClustersIndexData(A.t(), clusters);
std::ofstream myfile3;
myfile3.open("data after index.txt");
myfile3 << indexedData;
myfile3.close();
return indexedData;
}
static mat getKMeansClustersIndexData(mat A, mat centroids){
mat result=mat(A.n_cols, 1);
for(int i=0;i<A.n_cols;++i){
result(i, 0) = getIndexForClusterCentroids(A, i, centroids);
}
return result;
}
static int getIndexForClusterCentroids(mat A, int colIndex, mat centroids){
int index=1;
double lowestDistance=getEuclideanDistance(A, colIndex, centroids, 0);
for(int i=1;i<centroids.n_cols;++i){
double curDistance=getEuclideanDistance(A, colIndex, centroids, i);
if(curDistance<lowestDistance){
lowestDistance=curDistance;
index=i+1;
}
}
return index;
}
static double getEuclideanDistance(mat A, int colIndexA, mat B, int colIndexB){
double distance=0;
for(int i=0;i<A.n_rows;++i){
double elementA=A(i,colIndexA);
double elementB=B(i,colIndexB);
double diff=elementA-elementB;
distance+=diff*diff;
}
return sqrt(distance);
}
static mat getSqrtMat(mat A){
for(int i=0;i<A.n_rows;++i){
double curVal = A(i,i);
A(i,i) = sqrt(curVal);
}
return A;
}
static mat getSqrtMatDiag(mat A){
for(int i=0;i<A.n_rows;++i){
double curVal = A(i,i);
A(i,i) = sqrt(curVal);
}
return A;
}
static mat invertDiagMatrix(mat A){
for(int i=0;i<A.n_rows;++i){
double curVal = A(i,i);
A(i,i) = 1/curVal;
}
return A;
}
};
#endif
#ifndef BA_SYSTEM_COLLISIONDETECTION
#define BA_SYSTEM_COLLISIONDETECTION
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "armadillo.h"
#include "types/ba_struct_Rectangle.h"
#include "ba_system_collisionDetection_singleSetCompareRect.h"
#include "ba_system_collisionDetection_rectIntersection_1_.h"
#include "ba_system_collisionDetection_multiOr.h"
using namespace arma;
class ba_system_collisionDetection{
const int n = 2;
const int x = 1;
public:
ba_struct_Rectangle hulls[2];
bool collision;
ba_system_collisionDetection_singleSetCompareRect singleSetCompareRect;
ba_system_collisionDetection_rectIntersection_1_ rectIntersection[1];
ba_system_collisionDetection_multiOr multiOr;
void init()
{
singleSetCompareRect.init();
rectIntersection[0].init();
multiOr.init();
}
void execute()
{
singleSetCompareRect.setIn[0] = hulls[0];
singleSetCompareRect.setIn[1] = hulls[1];
singleSetCompareRect.execute();
rectIntersection[0].rect1 = singleSetCompareRect.outA[0];
rectIntersection[0].rect2 = singleSetCompareRect.outB[0];
rectIntersection[0].execute();
multiOr.boolIn[0] = rectIntersection[0].collision;
multiOr.execute();
collision = multiOr.boolOut;
}
};
#endif
#ifndef BA_SYSTEM_COLLISIONDETECTION_MULTIOR
#define BA_SYSTEM_COLLISIONDETECTION_MULTIOR
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "armadillo.h"
using namespace arma;
class ba_system_collisionDetection_multiOr{
const int x = 1;
public:
bool boolIn[1];
bool boolOut;
void init()
{
}
void execute()
{
bool flag = false;
for( auto i=1;i<=x;++i){
flag = flag||boolIn[i-1];
}
boolOut = flag;
}
};
#endif
#ifndef BA_SYSTEM_COLLISIONDETECTION_RECTINTERSECTION_1_
#define BA_SYSTEM_COLLISIONDETECTION_RECTINTERSECTION_1_
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "armadillo.h"
#include "types/ba_struct_Rectangle.h"
#include "ba_system_collisionDetection_rectIntersection_1__rectToLines1.h"
#include "ba_system_collisionDetection_rectIntersection_1__rectToLines2.h"
#include "ba_system_collisionDetection_rectIntersection_1__dualSetCompareLines.h"
#include "ba_system_collisionDetection_rectIntersection_1__lineIntersection_1_.h"
#include "ba_system_collisionDetection_rectIntersection_1__multiOrRect.h"
using namespace arma;
class ba_system_collisionDetection_rectIntersection_1_{
public:
ba_struct_Rectangle rect1;
ba_struct_Rectangle rect2;
bool collision;
ba_system_collisionDetection_rectIntersection_1__rectToLines1 rectToLines1;
ba_system_collisionDetection_rectIntersection_1__rectToLines2 rectToLines2;
ba_system_collisionDetection_rectIntersection_1__dualSetCompareLines dualSetCompareLines;
ba_system_collisionDetection_rectIntersection_1__lineIntersection_1_ lineIntersection[10];
ba_system_collisionDetection_rectIntersection_1__multiOrRect multiOrRect;
void init()
{
rectToLines1.init();
rectToLines2.init();
dualSetCompareLines.init();
lineIntersection[0].init();
lineIntersection[1].init();
lineIntersection[2].init();
lineIntersection[3].init();
lineIntersection[4].init();
lineIntersection[5].init();
lineIntersection[6].init();
lineIntersection[7].init();
lineIntersection[8].init();
lineIntersection[9].init();
multiOrRect.init();
}
void execute()
{
rectToLines1.rect = rect1;
rectToLines1.execute();
rectToLines2.rect = rect2;
rectToLines2.execute();
dualSetCompareLines.setInA[0] = rectToLines1.lines[0];
dualSetCompareLines.setInA[1] = rectToLines1.lines[1];
dualSetCompareLines.setInA[2] = rectToLines1.lines[2];
dualSetCompareLines.setInA[3] = rectToLines1.lines[3];
dualSetCompareLines.setInB[0] = rectToLines2.lines[0];
dualSetCompareLines.setInB[1] = rectToLines2.lines[1];
dualSetCompareLines.setInB[2] = rectToLines2.lines[2];
dualSetCompareLines.setInB[3] = rectToLines2.lines[3];
dualSetCompareLines.execute();
lineIntersection[0].lineA = dualSetCompareLines.outA[0];
lineIntersection[0].lineB = dualSetCompareLines.outB[0];
lineIntersection[0].execute();
lineIntersection[1].lineA = dualSetCompareLines.outA[1];
lineIntersection[1].lineB = dualSetCompareLines.outB[1];
lineIntersection[1].execute();
lineIntersection[2].lineA = dualSetCompareLines.outA[2];
lineIntersection[2].lineB = dualSetCompareLines.outB[2];
lineIntersection[2].execute();
lineIntersection[3].lineA = dualSetCompareLines.outA[3];
lineIntersection[3].lineB = dualSetCompareLines.outB[3];
lineIntersection[3].execute();
lineIntersection[4].lineA = dualSetCompareLines.outA[4];
lineIntersection[4].lineB = dualSetCompareLines.outB[4];
lineIntersection[4].execute();
lineIntersection[5].lineA = dualSetCompareLines.outA[5];
lineIntersection[5].lineB = dualSetCompareLines.outB[5];
lineIntersection[5].execute();
lineIntersection[6].lineA = dualSetCompareLines.outA[6];
lineIntersection[6].lineB = dualSetCompareLines.outB[6];
lineIntersection[6].execute();
lineIntersection[7].lineA = dualSetCompareLines.outA[7];
lineIntersection[7].lineB = dualSetCompareLines.outB[7];
lineIntersection[7].execute();
lineIntersection[8].lineA = dualSetCompareLines.outA[8];
lineIntersection[8].lineB = dualSetCompareLines.outB[8];
lineIntersection[8].execute();
lineIntersection[9].lineA = dualSetCompareLines.outA[9];
lineIntersection[9].lineB = dualSetCompareLines.outB[9];
lineIntersection[9].execute();
multiOrRect.boolIn[0] = lineIntersection[0].intersects;
multiOrRect.boolIn[1] = lineIntersection[1].intersects;
multiOrRect.boolIn[2] = lineIntersection[2].intersects;
multiOrRect.boolIn[3] = lineIntersection[3].intersects;
multiOrRect.boolIn[4] = lineIntersection[4].intersects;
multiOrRect.boolIn[5] = lineIntersection[5].intersects;
multiOrRect.boolIn[6] = lineIntersection[6].intersects;
multiOrRect.boolIn[7] = lineIntersection[7].intersects;
multiOrRect.boolIn[8] = lineIntersection[8].intersects;
multiOrRect.boolIn[9] = lineIntersection[9].intersects;
multiOrRect.execute();
collision = multiOrRect.boolOut;
}
};
#endif
#ifndef BA_SYSTEM_COLLISIONDETECTION_RECTINTERSECTION_1__DUALSETCOMPARELINES
#define BA_SYSTEM_COLLISIONDETECTION_RECTINTERSECTION_1__DUALSETCOMPARELINES
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "armadillo.h"
using namespace arma;
class ba_system_collisionDetection_rectIntersection_1__dualSetCompareLines{
const int rows = 2;
const int cols = 4;
const int n = 4;
const int n2 = 10;
public:
mat setInA[4];
mat setInB[4];
mat outA[10];
mat outB[10];
void init()
{
setInA[0]=mat(rows,cols);
setInA[1]=mat(rows,cols);
setInA[2]=mat(rows,cols);
setInA[3]=mat(rows,cols);
setInB[0]=mat(rows,cols);
setInB[1]=mat(rows,cols);
setInB[2]=mat(rows,cols);
setInB[3]=mat(rows,cols);
outA[0]=mat(rows,cols);
outA[1]=mat(rows,cols);
outA[2]=mat(rows,cols);
outA[3]=mat(rows,cols);
outA[4]=mat(rows,cols);
outA[5]=mat(rows,cols);
outA[6]=mat(rows,cols);
outA[7]=mat(rows,cols);
outA[8]=mat(rows,cols);
outA[9]=mat(rows,cols);
outB[0]=mat(rows,cols);
outB[1]=mat(rows,cols);
outB[2]=mat(rows,cols);
outB[3]=mat(rows,cols);
outB[4]=mat(rows,cols);
outB[5]=mat(rows,cols);
outB[6]=mat(rows,cols);
outB[7]=mat(rows,cols);
outB[8]=mat(rows,cols);
outB[9]=mat(rows,cols);
}
void execute()
{
int counter = 1;
for( auto i=1;i<=n;++i){
for( auto j=i;j<=n;++j){
outA[counter-1] = setInA[i-1];
outB[counter-1] = setInB[j-1];
counter = counter+1;
}
}
}
};
#endif
#ifndef BA_SYSTEM_COLLISIONDETECTION_RECTINTERSECTION_1__LINEINTERSECTION_1_
#define BA_SYSTEM_COLLISIONDETECTION_RECTINTERSECTION_1__LINEINTERSECTION_1_
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#include "armadillo.h"
#include <cmath>
using namespace arma;
class ba_system_collisionDetection_rectIntersection_1__lineIntersection_1_{
public:
mat lineA;
mat lineB;
mat point;
bool intersects;
void init()
{
lineA=mat(4,1);
lineB=mat(4,1);
point=mat(2,1);
}
void execute()
{
double Ax = lineA(3-1, 1-1)-lineA(1-1, 1-1);
double Ay = lineA(4-1, 1-1)-lineA(2-1, 1-1);
double Bx = lineB(1-1, 1-1)-lineB(3-1, 1-1);
double By = lineB(2-1, 1-1)-lineB(4-1, 1-1);
double Cx = lineA(1-1, 1-1)-lineB(1-1, 1-1);
double Cy = lineA(2-1, 1-1)-lineB(2-1, 1-1);
double n1 = Ay*Bx-Ax*By;
double cutoff = 1.0E-8;
bool res = false;
if(((std::abs(n1) > cutoff))){
double alpha = (By*Cx-Bx*Cy)/n1;
if(((alpha >= cutoff))&&((alpha <= 1))){
double beta = (Ax*Cy-Ay*Cx)/n1;
if(((beta >= cutoff))&&((beta <= 1))){
res = true;
point(1-1, 1-1) = lineA(1-1, 1-1)+alpha*Ax;
point(2-1, 1-1) = lineA(2-1, 1-1)+alpha*Ay;
}
}
}
intersects = res;
}
};
#endif
#ifndef BA_SYSTEM_COLLISIONDETECTION_RECTINTERSECTION_1__MULTIORRECT
#define BA_SYSTEM_COLLISIONDETECTION_RECTINTERSECTION_1__MULTIORRECT
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif