void initLogger(int networkId)
{
// Error file
if (gErrorFile.is_open())
{
gErrorFile.close();
gErrorFile.clear();
}
std::stringstream errorFile;
errorFile << "../error_log_" << networkId << ".txt";
gErrorFilePath = errorFile.str();
gErrorFile.open(gErrorFilePath.c_str());
// Debug message file
if (gDebugMessageFile.is_open())
{
gDebugMessageFile.close();
gDebugMessageFile.clear();
}
std::stringstream debugFile;
debugFile << "../debug_message_log_" << networkId << ".txt";
gDebugMessageFilePath = debugFile.str();
gDebugMessageFile.open(gDebugMessageFilePath.c_str());
}
void writeTrajectory(TrajectoryBasePtr ptraj){
mtx_.lock();
stringstream ss;
if (!!(ptraj)){
if ((ptraj->GetDuration() < timer)){
timer = ptraj->GetDuration();
//std::cout << "Storing Trajectory with following info" << std::endl;
//std::cout << timer << std::endl;
ptraj->serialize(ss);
traj.open("traj.xml");
traj.clear();
traj << ss.str();
traj.close();
}
else {
//std::cout << "Ignoring Trajectory Files" << std::endl;
}
}
else {
//std::cout << "Planning Failed : trajectory empty" << std::endl;
}
mtx_.unlock();
}
std::ostream& LoggerImpl::getAppender()
{
if (pipe)
return *pipe;
else if (!fname.empty())
{
if (!outfile.is_open())
{
outfile.clear();
outfile.open(fname.c_str(), std::ios::out | std::ios::app);
counter.resetCount(outfile.tellp());
}
if (maxfilesize > 0)
{
if (counter.getCount() > maxfilesize)
{
doRotate();
counter.resetCount();
}
return tee;
}
else
return outfile;
}
else if (loghost.isConnected())
return udpmessage;
else
return std::cerr;
}
void sotDebugTrace::openFile( const char * filename )
{
if( debugfile.good()&&debugfile.is_open() ) debugfile.close();
debugfile.clear();
debugfile.open( filename, std::ios::trunc&std::ios::out );
//std::cout << filename << debugfile.good() << debugfile.is_open() << std::endl;
}
void open(bool truncate)
{
if (open_filename == m_filename) return;
log_file.close();
log_file.clear();
log_file.open(m_filename.c_str(), truncate ? std::ios_base::trunc : std::ios_base::app);
open_filename = m_filename;
if (!log_file.good())
fprintf(stderr, "Failed to open logfile %s: %s\n", m_filename.c_str(), strerror(errno));
}
std::string promptUserForFile(std::ofstream &outFile, std::string prompt) {
while (true) {
std::cout << prompt;
std::string fileName;
getline(std::cin, fileName);
outFile.open(fileName.c_str());
if (!outFile.fail()) return fileName;
outFile.clear();
std::cout << "Unable to open that file. Try again." << std::endl;
if (prompt == "") prompt = "Output file: ";
}
}
bool FileSystem::rawopen
(
std::ofstream& out, // Output stream to open.
const string &fname, // May be converted to upper-case.
bool is_text // Should the file be opened in text mode
)
{
string name = fname;
if (!rewrite_virtual_path(name)) {
con.Print_err(MM_MAJOR_WARN, "Illegal file access\n");
return false;
}
#if defined(MACOS) || (__GNUG__ > 2)
std::ios_base::openmode mode = std::ios::out | std::ios::trunc;
if (!is_text) mode |= std::ios::binary;
#elif defined(UNIX)
int mode = std::ios::out | std::ios::trunc;
#else
int mode = std::ios::out | std::ios::trunc;
if (!is_text) mode |= std::ios::binary;
#endif
switch_slashes(name);
// We first "clear" the stream object. This is done to prevent
// problems when re-using stream objects
out.clear();
int uppercasecount = 0;
do {
out.open(name.c_str(), mode); // Try to open
if (out.good()) return true; // found it!
out.clear(); // Forget ye not
} while (base_to_uppercase(name, ++uppercasecount));
// file not found.
return false;
}
virtual void reset() {
DVLOG(5) << "resetting histogram";
if (!log_initialized) {
log_initialized = true;
char * jobid = getenv("SLURM_JOB_ID");
char fname[256]; sprintf(fname, "histogram.%s/%s.%d.out", jobid, name, mycore());
log.open(fname, std::ios::out | std::ios_base::binary);
DVLOG(5) << "opened " << fname;
}
if (log_initialized) {
log.clear();
log.seekp(0, std::ios::beg);
}
value_ = nil_value;
}
void parse_template(template_files_vector::iterator i, std::ifstream &input, std::ofstream &output, const symbol_map &symbols)
{
std::string line;
parser_vector parsers;
parsers.push_back(parser_object_ptr(new symbol_parser(symbols, parsers, input, output)));
input.open(i->first.c_str(), std::ios::in);
string_pair &out_file = i->second;
output.open(std::string(out_file.first + symbols.find("NAME_LOWER")->second + out_file.second).c_str(),
std::ios::out | std::ios::trunc);
if( !input.is_open() )
throw std::runtime_error("Failed to open template file");
if( !output.is_open() )
throw std::runtime_error("Failed to open output file");
std::string buff;
// read and write line by line
while(true)
{
symbol_parser::m_should_output = true;
std::getline(input, line);
buff = line;
if( input.eof() )
break;
if( !input.good() || !output.good() )
throw std::runtime_error("Input or Output error!");
for( parser_vector::reverse_iterator i = parsers.rbegin(), end = parsers.rend(); i != end; ++i )
if( (**i)(buff) )
break;
}
input.close();
output.close();
input.clear();
output.clear();
}
//--- Open an output file.
//
// Note not under unit test.
//
bool openOutputFile(std::ofstream& fileStream, const char *const fileName, std::ios_base::openmode mode,
std::string& errStr)
{
//
// Start with a clean slate.
if( fileStream.is_open() )
fileStream.close();
fileStream.clear(std::ios_base::goodbit);
//
// Open file.
fileStream.open(fileName, mode);
if( fileStream.fail() )
{
if( 0 != errStr.size() )
errStr.push_back('\n');
errStr.append("Failed to open file '").append(fileName).push_back('\'');
return( 0 );
}
//
return( 1 );
} // End fcn openOutputFile().
void LoggerImpl::doRotate()
{
outfile.clear();
outfile.close();
// ignore unlink- and rename-errors. In case of failure the
// original file is reopened
std::string newfilename = mkfilename(maxbackupindex);
::unlink(newfilename.c_str());
for (unsigned idx = maxbackupindex; idx > 0; --idx)
{
std::string oldfilename = mkfilename(idx - 1);
::rename(oldfilename.c_str(), newfilename.c_str());
newfilename = oldfilename;
}
::rename(fname.c_str(), newfilename.c_str());
outfile.open(fname.c_str(), std::ios::out | std::ios::app);
counter.resetCount(outfile.tellp());
}
std::string promptUserForFile(std::ofstream& stream,
const std::string& prompt,
const std::string& reprompt) {
std::string promptCopy = prompt;
std::string repromptCopy = reprompt;
if (reprompt == "") {
repromptCopy = "Unable to open that file. Try again.";
}
appendSpace(promptCopy);
while (true) {
std::cout << promptCopy;
std::string filename;
getline(std::cin, filename);
if (!filename.empty()) {
openFile(stream, filename);
if (!stream.fail()) return filename;
stream.clear();
}
std::cout << repromptCopy << std::endl;
if (promptCopy == "") promptCopy = "Output file: ";
}
}
// visit generate_ham_cycles_helper
void generate_ham_cycle(int m, int n, int v)
{
int size = m * n;
int column = n;
int** matrix;
generate_grid_matrix(matrix, m, n);
///////////////////////////////////////////////////////
fout.open("cuda/matrix.in");
fout << " " << size << std::endl;
for(int i = 0; i < size; ++i){
for(int j = 0; j < size; ++j){
fout << std::setw(3) << matrix[i][j];
}
fout << std::endl;
}
fout.close();
fout.clear();
///////////////////////////////////////////////////////
//print adjacency matrix for grid graph
//for(int i = 0; i < size; ++i){
// for(int j = 0; j < size; ++j){
// std::cout << matrix[i][j] << ' ';
// //std::cout << matrix[i][j] << ", ";
// }
// std::cout << std::endl;
//}
///////////////////////////////////////////////////////
bool* visit; // = {0,};
visit = new bool[size];
for(int i = 0; i < size; ++i){
visit[i] = false;
}
int w = v - 1; // starting node of hamiltonian cycle
std::vector<int> myvector;
myvector.push_back(w); // start of hamiltonian cycle
std::cout << std::endl;
generate_ham_cycle_helper(w, w, myvector, matrix, visit, size, column);
std::cout << " Count of HAMILTONIAN CYCLES " << cycles.size() << std::endl << std::endl;
//std::cout << "\nCount of HAMILTONIAN CYCLES = "
// << generate_ham_cycle_helper(w, w, myvector, matrix, visit, size, column)
// << std::endl << std::endl;
//print all hamiltonian cycles
//for(int i = 0; i < cycles.size(); ++i){
// print_vector(cycles[i]);
//}
int s = cycles.size();
if(s) {
int t,c1,c2;
int min = distance(0, 0);
long unsigned int sum1 = 0;
long unsigned int sum2 = 0;
std::vector<int> f1;
std::vector<int> f2;
c1 = 0;
c2 = 0;
//variables for non simetrical cycles
bool b = true;
unsigned int count = 0;
fout.open("octave/functions.in");
for(int i = 0; i < s; ++i){
//sum and f1 functions of C1 cycle
sum1 = charac_function(cycles[i], f1, column);
for(int j = i + 1; j < s; ++j){
//sum and f2 functions of C2 cycle
sum2 = charac_function(cycles[j], f2, column);
if(if_equal(f1, f2)){
b = false;
// print cycles, which function are equaly
// print_function(f1);
// print_vector_in_grid(cycles[i], column);
// print_vector_in_grid(cycles[j], column);
}
t = distance(i,j);
if(min > t){
min = t;
c1 = i;
c2 = j;
}
}
if(b) {
//print non charac. functions to file functions.in
print_function_to_file(f1);
++count;
}
b = true;
}
fout.close();
fout.clear();
std::cout << " Count of non simetrical HAMILTONIAN CYCLES "
<< count << std::endl << std::endl;
std::cout << " nm " << size << std::endl;
std::cout << " min distance " << min << std::endl;
std::cout << " max distance " << 2 * size - 2 * min << std::endl;
//print_vector(cycles[c1]);
//print_vector(cycles[c2]);
print_vector_in_grid(cycles[c1], column);
print_vector_in_grid(cycles[c2], column);
//for(int i = 0; i < s; ++i){
// for(int j = i + 1; j < s; ++j){
// t = distance(i,j);
// if(min == t){
// std::cout << "/////////////////////////////////////////////////////////////" << std::endl;
// print_vector_in_grid(cycles[i], column);
// print_vector_in_grid(cycles[j], column);
// }
// }
//}
}
///////////////////////////////////////////////////////
//std::vector<int> v1;
//std::vector<int> v2;
//std::vector<int> f1;
//std::vector<int> f2;
//v1.push_back(0);v1.push_back(1);v1.push_back(9);v1.push_back(17);v1.push_back(18);v1.push_back(10);v1.push_back(2);v1.push_back(3);v1.push_back(11);v1.push_back(12);v1.push_back(4);v1.push_back(5);v1.push_back(6);v1.push_back(7);v1.push_back(15);v1.push_back(23);v1.push_back(31);v1.push_back(30);v1.push_back(22);v1.push_back(14);v1.push_back(13);v1.push_back(21);v1.push_back(29);v1.push_back(28);v1.push_back(20);v1.push_back(19);v1.push_back(27);v1.push_back(26);v1.push_back(25);v1.push_back(24);v1.push_back(16);v1.push_back(8);
//v2.push_back(0);v2.push_back(1);v2.push_back(2);v2.push_back(3);v2.push_back(11);v2.push_back(12);v2.push_back(4);v2.push_back(5);v2.push_back(6);v2.push_back(7);v2.push_back(15);v2.push_back(23);v2.push_back(31);v2.push_back(30);v2.push_back(22);v2.push_back(14);v2.push_back(13);v2.push_back(21);v2.push_back(29);v2.push_back(28);v2.push_back(20);v2.push_back(19);v2.push_back(27);v2.push_back(26);v2.push_back(18);v2.push_back(10);v2.push_back(9);v2.push_back(17);v2.push_back(25);v2.push_back(24);v2.push_back(16);v2.push_back(8);
//int sum1 = charac_function(v1, f1, 8);
//int sum2 = charac_function(v2, f2, 8);
//print_function(f1);
//print_function(f2);
//std::cout << "sum1 = " << sum1 << std::endl;
//std::cout << "sum2 = " << sum2 << std::endl;
//print_vector_in_grid(v1, 8);
//print_vector_in_grid(v2, 8);
///////////////////////////////////////////////////////
std::cout << " Generated" << std::endl;
std::cout << " Adjacency matrix to file cuda/matrix.in" << std::endl;
std::cout << " Characteristic functions of loops to file octave/functions.in" << std::endl;
//generate Kirchhorff matrix
for(int i = 0; i < size; ++i){
for(int j = 0; j < size; ++j){
matrix[i][j] = -matrix[i][j];
}
}
matrix[0][n] = -1;
matrix[0][0] = 2;
matrix[n - 1][n - 1] = 2;
matrix[size - n][size - n] = 2;
matrix[size - 1][size - 1] = 2;
for(int i = 1; i < n-1; ++i){
matrix[i][i] = 3;
matrix[size - i - 1][size - i - 1] = 3;
}
for(int i = 1; i < m - 1; ++i){
matrix[i * n][i * n] = 3;
matrix[n + i * n - 1][n + i * n - 1] = 3;
}
for(int i = 1; i < m - 1; ++i){
for(int j = 1; j < n - 1; ++j){
matrix[i * n + j][i * n + j] = 4;
}
}
fout.open("octave/matrix.in");
for(int i = 0; i < size; ++i){
for(int j = 0; j < size; ++j){
fout << std::setw(3) << matrix[i][j];
}
fout << std::endl;
}
fout.close();
fout.clear();
std::cout << " Kirchhorff matrix to file octave/matrix.in" << std::endl << std::endl;
///////////////////////////////////////////////////////
for(int i = 0; i < size; ++i){
delete [] matrix[i];
}
delete [] matrix;
}
/** Open file stream to write in */
static void openTextFileStream(std::ofstream& fileStream,
const std::string& fileName) {
fileStream.open(fileName, std::ios::out);
assert_true(fileStream.is_open());
fileStream.clear();
}
void seekwriting(unsigned long int& seekpos) {
ofs_.clear();
ofs_.seekp(B_HEADERSIZE+seekpos);
}
void GLXOSD::stopFrameLogging() {
Lock lock(&frameLogMutex);
frameLoggingEnabled = false;
frameLogStream.close();
frameLogStream.clear();
}
int main()
{
traj.clear();
unsigned int mainthreadsleft = numThreads;
// boost::shared_ptr<boost::thread> ( new boost::thread(boost::bind( &track_threads )));
Ta.trans = transA;
Ta.rot = quatA;
Tb.trans = transB;
Tb.rot = quatB;
std::string scenefilename = "scenes/test6dof.mujin.zae";
std::string viewername = "qtcoin";
RaveInitialize(true); // start openrave core
EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
Transform robot_t;
RaveVector< dReal > transR(c, d, 0);
robot_t.trans = transR;
//boost::thread thviewer(boost::bind(SetViewer,penv,viewername));
penv->Load(scenefilename);
RobotBasePtr probot = penv->GetRobot("RV-4F");
//removing floor for collision checking
EnvironmentBasePtr pclondedenv = penv->CloneSelf(Clone_Bodies);
pclondedenv->Remove( pclondedenv->GetKinBody("floor"));
RobotBasePtr probot_clone = pclondedenv->GetRobot("RV-4F");
unsigned int tot = ((( abs(a) + abs(c) )/discretization_x )+1) * (((( abs(b) + abs(d) )/discretization_y )+1) * (( abs( z )/discretization_z )+1));
unsigned int tot_o = tot;
for (unsigned int i = 0 ; i <= (( abs(a) + abs(c) )/discretization_x );i++) {
for (unsigned int j = 0 ; j <= (( abs(b) + abs(d) )/discretization_y ); j++) {
for (unsigned int k = 0 ; k <= ( abs( z )/discretization_z ) ; k++) {
////std::cout << transR[0] << ";" << transR[1] << ";" << transR[2] << std::endl;
//boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
robot_t.trans = transR;
probot->SetTransform(robot_t);
probot_clone->SetTransform(robot_t);
if( pclondedenv->CheckCollision(RobotBaseConstPtr(probot_clone)) ){
//std::cout << "Robot in collision with the environment" << std::endl;
}
else {
do_task(Ta, Tb, penv,3);
}
tot -= 1;
std::cout << tot << "/" << tot_o << std::endl;
transR[2] = transR[2]+ discretization_z;
}
transR[2] = 0;
transR[1] = transR[1] + discretization_y;
robot_t.trans = transR;
}
transR[2] = 0;
transR[1] = c;
transR[0] = transR[0] + discretization_x;
robot_t.trans = transR;
}
//thviewer.join(); // wait for the viewer thread to exit
RaveDestroy(); // make sure to destroy the OpenRAVE runtime
penv->Destroy(); // destroy
return 0;
}
void CreateConfig(std::ofstream &file, Direct3DCap &cap)
{
file.clear();
//onderstaande code vraagt om scherm
int i = 1;
std::cout << "Choose one of the above screens" << std::endl;
scanf("%d", &i);
while (i >= cap.return_adapterCounnt() || i < 0)
{
std::cout << "Choice: " << i << " is invalid \nChoose one of the above screens" << std::endl;
scanf("%d", &i);
}
file << i << std::endl;
i = 0;
std::cout << "How many percent should be captured from the top/bottom (0-100)" << std::endl;
scanf("%d", &i);
while (i < 1 || i > 100)
{
std::cout << "Choice: " << i << " is invalid \n" << std::endl;
scanf("%d", &i);
}
file << i << std::endl;
i = 0;
std::cout << "How many percent should be captured from the left/right side (0-100)" << std::endl;
scanf("%d", &i);
while (i < 1 || i > 100)
{
std::cout << "Choice: " << i << " is invalid \n" << std::endl;
scanf("%d", &i);
}
file << i << std::endl;
int j = 0;
std::cout << "Which COM port is the arduino on" << std::endl;
scanf("%d", &j);
while (j < 0 || j > 60)
{
std::cout << "Choice: " << i << " is invalid \n" << std::endl;
scanf("%d", &j);
}
std::string String;
String = "\\\\.\\COM";
String += std::to_string(j);
char *temp = new char[String.size() + 1];
std::copy(String.begin(), String.end(), temp);
temp[String.size()] = '\0';
int *pointer = LedAmountTest(temp);
file << pointer[0] << std::endl;
file << pointer[1] << std::endl;
file << pointer[2] << std::endl;
file << pointer[3] << std::endl;
i = 0;
std::cout << "What is the minimum black treshold (0- 60)" << std::endl;
scanf("%d", &i);
while (i < 1 || i > 60)
{
std::cout << "Choice: " << i << " is invalid \n" << std::endl;
scanf("%d", &i);
}
file << i << std::endl;
i = 0;
file << j << std::endl;
}
bool openFile(std::ofstream& stream, const std::string& filename) {
stream.clear();
stream.open(expandPathname(filename).c_str());
return !stream.fail();
}
void PrintGMeshVTK(TPZGeoMesh * gmesh, std::ofstream &file)
{
file.clear();
int nelements = gmesh->NElements();
std::stringstream node, connectivity, type;
//Header
file << "# vtk DataFile Version 3.0" << std::endl;
file << "TPZGeoMesh VTK Visualization" << std::endl;
file << "ASCII" << std::endl << std::endl;
file << "DATASET UNSTRUCTURED_GRID" << std::endl;
file << "POINTS ";
int actualNode = -1, size = 0, nVALIDelements = 0;
for(int el = 0; el < nelements; el++)
{
if(gmesh->ElementVec()[el]->Type() == EPoint)//Exclude Lines and Arc3D
{
continue;
}
if(gmesh->ElementVec()[el]->Type() == EOned)//Exclude Lines and Arc3D
{
continue;
}
if(gmesh->ElementVec()[el]->HasSubElement())
{
continue;
}
int elNnodes = gmesh->ElementVec()[el]->NNodes();
size += (1+elNnodes);
connectivity << elNnodes;
for(int t = 0; t < elNnodes; t++)
{
for(int c = 0; c < 3; c++)
{
double coord = gmesh->NodeVec()[gmesh->ElementVec()[el]->NodeIndex(t)].Coord(c);
node << coord << " ";
}
node << std::endl;
actualNode++;
connectivity << " " << actualNode;
}
connectivity << std::endl;
int elType = -1;
switch (gmesh->ElementVec()[el]->Type())
{
case (ETriangle):
{
elType = 5;
break;
}
case (EQuadrilateral ):
{
elType = 9;
break;
}
case (ETetraedro):
{
elType = 10;
break;
}
case (EPiramide):
{
elType = 14;
break;
}
case (EPrisma):
{
elType = 13;
break;
}
case (ECube):
{
elType = 12;
break;
}
default:
{
//ElementType NOT Found!!!
DebugStop();
break;
}
}
type << elType << std::endl;
nVALIDelements++;
}
node << std::endl;
actualNode++;
file << actualNode << " float" << std::endl << node.str();
file << "CELLS " << nVALIDelements << " ";
file << size << std::endl;
file << connectivity.str() << std::endl;
file << "CELL_TYPES " << nVALIDelements << std::endl;
file << type.str();
file.close();
}
