<commit_before>#include "includes_defines.h"
#include "types.h"
#include "global.h"
#include "utilities.cpp"
#include "load_scenarios.cpp"
#include "heuristic_function.cpp"
#include "custom_encoding.cpp"
#include "encoding_methods.cpp"

extern "C" {

	int get_bit(int row, int col){
		return ((int) opcodes[row][col]);
	}

	int get_codes_length(){
		return bits;
	}

	int load_graphs_codes(char *file_in,
			char *custom_file_name){

		FILE *fp;
		int trivial = 0;
		int err=0;
		int elements;
		int min_disp;

		if(first){
			first = FALSE;
			if(temporary_files_creation() != 0){
				return -1;
			}
		}

		if( (fpLOG = fopen(LOG,"w")) == NULL){
			fprintf(stderr,"Error on opening LOG file for writing.\n");
		}
		fprintf(fpLOG,WELCOME_STRING);

		// memory allocation
		fprintf(fpLOG,"Allocating memory for vertex names and graphs...");
		g = (GRAPH_TYPE *) malloc(sizeof(GRAPH_TYPE) * scenariosLimit);
		fprintf(fpLOG,"DONE\n");
		fflush(stdout);

		//**********************************************************************
		// Building CPOG Part
		//**********************************************************************

		// loading scenarios
		fprintf(fpLOG,"\nOptimal scenarios encoding and CPOG synthesis.\n");
		if(loadScenarios(file_in, fp) != 0){
			fprintf(stderr,"Loading scenarios failed.\n");
			return -1;
		}
		fprintf(fpLOG,"\n%d scenarios have been loaded.\n", n);

		// looking for predicates
		if(predicateSearch() != 0){
			fprintf(stderr,"Predicate searching failed.\n");
			return -1;
		}

		// looking for non-trivial constraints
		if( (fp = fopen(CONSTRAINTS_FILE,"w")) == NULL){
			fprintf(stderr,"Error on opening constraints file for writing.\n");
			return -1;
		}
		if(nonTrivialConstraints(fp, &total, &trivial) != 0){
			fprintf(stderr,"Non-trivial constraints searching failed.\n");
			return -1;
		}
		fprintf(fpLOG,"\n%d non-trivial encoding constraints found:\n\n", total - trivial);

		// writing non-trivial constraints into a file
		if( (fp = fopen(TRIVIAL_ENCODING_FILE,"w")) == NULL){
			fprintf(stderr,"Error on opening constraints file for writing.\n");
			return -1;
		}
		for(int i = 0; i < total; i++)
			if (!encodings[i].trivial) {
				fprintf(fp,"%s\n",encodings[i].constraint.c_str());
		}
		fclose(fp);

		fprintf(fpLOG,"\nBuilding conflict graph... ");
		if(conflictGraph(&total) != 0){
			fprintf(stderr,"Building conflict graph failed.\n");
			return -1;
		}
		fprintf(fpLOG,"DONE.\n");

		//**********************************************************************
		// Reading encoding set by the user
		//**********************************************************************

		fprintf(fpLOG,"Reading encodings set... ");
		fflush(stdout);
		if(read_set_encoding(custom_file_name,n,&bits) != 0){
			fprintf(stderr,"Error on reading encoding set.\n");
			removeTempFiles();
			return -1;
		}
		fprintf(fpLOG,"DONE\n");

		fprintf(fpLOG,"Check correcteness of encoding set... ");
		if(check_correctness(custom_file_name,n,tot_enc,bits) != 0){
			removeTempFiles();
			return -1;
		}
		fprintf(fpLOG,"DONE\n");

		//**********************************************************************
		// Variable preparation for encoding
		//**********************************************************************
		strcpy(file_in,TRIVIAL_ENCODING_FILE);
		file_cons = strdup(CONSTRAINTS_FILE);

		/*READ NON-TRIVIAL ENCODING FILE*/
		fprintf(fpLOG,"Reading non-trivial encoding file... ");
		if( (err = read_file(file_in)) ){
			fprintf(stderr,"Error occured while reading non-trivial encoding file, error code: %d", err);
			removeTempFiles();
			return -1;
		}
		fprintf(fpLOG,"DONE\n");

		/*SEED FOR RAND*/
		srand(time(NULL));

		/*ALLOCATING AND ZEROING DIFFERENCE MATRIX*/
		opt_diff = (int**) calloc(n, sizeof(int*));
		for(int i=0;i<n;i++)
			opt_diff[i] = (int*) calloc(n, sizeof(int));

		/*NUMBER OF POSSIBLE ENCODING*/
		tot_enc = 1;
		for(int i=0;i<bits;i++) tot_enc *= 2;

		/*ANALYSIS IF IT'S A PERMUTATION OR A DISPOSITION*/
		num_perm = 1;
		if (n == tot_enc){
			/*PERMUTATION*/
			if(!unfix && !SET){
				for(int i = 1; i< tot_enc; i++)
					num_perm *= i;
			}else{
				for(int i = 1; i<= tot_enc; i++)
					num_perm *= i;
			}
			fprintf(fpLOG,"Number of possible permutations by fixing first element: %lld\n", num_perm);
		}
		else{
			/*DISPOSITION*/
			if(!unfix && !SET){
				elements = tot_enc-1;
				min_disp = elements - (n- 1) + 1;
			}else{
				elements = tot_enc;
				min_disp = elements - (n) + 1;
			}
				num_perm = 1;
			for(int i=elements; i>= min_disp; i--)
				num_perm *= i;
			fprintf(fpLOG,"Number of possible dispositions by fixing first element: %lld\n", num_perm);
		}

		all_perm = num_perm;
//		num_perm = EXHAUSTIVE_MAX_PERM;

		/*PREPARATION DATA FOR ENCODING PERMUTATIONS*/
		enc = (int*) calloc(tot_enc, sizeof(int));

		/*First element is fixed*/
		if (!unfix && !SET)
			enc[0] = 1;

		sol = (int*) calloc(tot_enc, sizeof(int));
		if (sol == NULL){
			fprintf(stderr,"solution variable = null\n");
			removeTempFiles();
			return -1;
		}

		/*BUILDING DIFFERENCE MATRIX*/
		fprintf(fpLOG,"Building DM (=Difference Matrix)... ");
		if( (err = difference_matrix(cpog_count)) ){
			fprintf(stderr,"Error occurred while building difference matrix, error code: %d", err);
			removeTempFiles();
			return -1;
		}
		fprintf(fpLOG,"DONE\n");

		fclose(fpLOG);
		removeTempFiles();

		return 0;
	}

	int unload_graphs_codes(){
		if(g != NULL) free(g);
		if(manual_file != NULL) free(manual_file);
		if(manual_file_back != NULL) free(manual_file_back);
		if(custom_perm != NULL) free(custom_perm);
		if(custom_perm_back != NULL) free(custom_perm_back);
		if(opt_diff != NULL) {
			for(int i = 0; i<cpog_count; i++)
				if(opt_diff[i] != NULL) free(opt_diff[i]);
			free(opt_diff);
		}
		if(perm != NULL) {
			for(long long int i = 0; i<num_perm; i++)
				if(perm[i] != NULL )free(perm[i]);
			free(perm);
		}
		if(file_cons != NULL) free(file_cons);
		if(weights != NULL) free(weights);
		if(diff != NULL){
			for(int i = 0; i< len_sequence; i++)
				if(diff[i] != NULL) free(diff[i]);
			free(diff);
		}
		if(opcodes != NULL){
			for(int i = 0; i < cpog_count; i++)
				if(opcodes[i] != NULL) free(opcodes[i]);
		}
		if(enc != NULL) free(enc);
		if(sol != NULL) free(sol);
		cpog_count = 0;
		n = 0;
		len_sequence = 0;

		// Andrey's tool
		if( !eventNames.empty() ) eventNames.clear();
		for(int i = 0; i<eventsLimit; i++)
			if( !eventNames_str[i].empty() )eventNames_str[i].clear();
		for(int i = 0; i<eventsLimit;i++)
			if( !eventPredicates[i].empty() )
				eventPredicates[i].clear();
		if( !scenarioNames.empty() )scenarioNames.clear();
		if( !scenarioOpcodes.empty() )scenarioOpcodes.clear();
		for(int i = 0; i< eventsLimit; i++)
			for(int j = 0; j< predicatesLimit; j++)
				if(!ev[i][j].empty()) ev[i][j].clear();
		for(int i = 0; i< eventsLimit; i++)
			for(int j = 0; j< eventsLimit; j++)
				if(!ee[i][j].empty()) ee[i][j].clear();

		if(!constraints.empty()) constraints.clear();
		if ( !encodings.empty() )encodings.clear();
		if ( !cgv.empty() ) cgv.clear();
		if ( !cge.empty() ) cge.clear();
		if ( !literal.empty() ) literal.clear();
		if ( !bestLiteral.empty() ) bestLiteral.clear();

		for(int i = 0; i< eventsLimit; i++)
			for(int j = 0; j< predicatesLimit; j++)
				if(!vConditions[i][j].empty())
					vConditions[i][j].clear();

		for(int i = 0; i< eventsLimit; i++)
			for(int j = 0; j< eventsLimit; j++)
				if(!aConditions[i][j].empty())
					aConditions[i][j].clear();
		return 0;
	}

	int single_literal_encoding(){

		if( (fpLOG = fopen(LOG,"a")) == NULL){
			fprintf(stderr,"Error on opening LOG file for appending.\n");
		}

		fprintf(fpLOG,"Running single-literal encoding.\n");

		if(singleLiteralEncoding(total) != 0){
			fprintf(stderr,"Single-literal encoding failed.\n");
			return -1;
		}

		if (export_variables(single_literal) != 0){
			return -1;
		}

		fclose(fpLOG);

		return 0;
	}

	int sequential_encoding(){

		if( (fpLOG = fopen(LOG,"a")) == NULL){
			fprintf(stderr,"Error on opening LOG file for appending.\n");
		}

		fprintf(fpLOG,"Running Sequential encoding.\n");

		if(allocate_encodings_space(1) != 0){
			fprintf(stderr,"Sequential encoding failed.\n");
			return -1;
		}

		if(sequentialEncoding() != 0){
			fprintf(stderr,"Sequential encoding failed.\n");
			return -1;
		}

		if (export_variables(sequential) != 0){
			return -1;
		}

		fclose(fpLOG);

		return 0;
	}

	int random_encoding(int num_enc){

		if( (fpLOG = fopen(LOG,"a")) == NULL){
			fprintf(stderr,"Error on opening LOG file for appending.\n");
		}

		bits = bits_saved;

		fprintf(fpLOG,"Running Random encoding.\n");

		if(allocate_encodings_space(num_enc) != 0){
			fprintf(stderr,"Encoding allocation failed.\n");
			return -1;
		}

		if(randomEncoding() != 0){
			fprintf(stderr,"Random encoding failed.\n");
			return -1;
		}

		if(heuristic_choice() != 0) {
			fprintf(stderr,"Computing heuristic encoding.\n");
			return -1;
		}

		if (export_variables(randomE) != 0){
			return -1;
		}

		fclose(fpLOG);

		return 0;
	}

	int heuristic_encoding(int num_enc){

		if( (fpLOG = fopen(LOG,"a")) == NULL){
			fprintf(stderr,"Error on opening LOG file for appending.\n");
		}

		bits = bits_saved;

		if(allocate_encodings_space(num_enc) != 0){
			fprintf(stderr,"Encoding allocation failed.\n");
			return -1;
		}

		fprintf(fpLOG,"Running Random generation... ");

		if(randomEncoding() != 0){
			fprintf(stderr,"Random encoding failed.\n");
			return -1;
		}
		fprintf(fpLOG,"DONE.\n");

		fprintf(fpLOG,"Running Heuristic optimisation... ");
		if(start_simulated_annealing() != 0){
			fprintf(stderr,"Heuristic optimisation failed.\n");
			return -1;
		}
		fprintf(fpLOG,"DONE.\n");

		if(heuristic_choice() != 0) {
			fprintf(stderr,"Computing heuristic encoding.\n");
			return -1;
		}

		if (export_variables(heuristic) != 0){
			return -1;
		}

		fclose(fpLOG);

		return 0;
	}

	int exhaustive_encoding(int num_enc){

		if( (fpLOG = fopen(LOG,"a")) == NULL){
			fprintf(stderr,"Error on opening LOG file for appending.\n");
		}

		if(allocate_encodings_space(num_enc) != 0){
			fprintf(stderr,"Encoding allocation failed.\n");
			return -1;
		}

		fprintf(fpLOG,"Running Exhaustive encoding.\n");

		if(!unfix && !SET){
			//permutation_stdalgo(cpog_count,tot_enc);
			fprintf(fpLOG,"Permutation algorithm unconstrained... ");
			exhaustiveEncoding(sol,0,enc,cpog_count, tot_enc);
			fprintf(fpLOG,"DONE\n");
		}else{
			fprintf(fpLOG,"Permutation algorithm constrained... ");
			exhaustiveEncoding(sol,-1,enc,cpog_count, tot_enc);
			fprintf(fpLOG,"DONE\n");

			fprintf(fpLOG,"Filtering encoding... ");
			filter_encodings(cpog_count, bits, tot_enc);
			fprintf(fpLOG,"DONE\n");

		}

		if(heuristic_choice() != 0) {
			fprintf(stderr,"Computing heuristic encoding.\n");
			return -1;
		}

		if (export_variables(exhaustive) != 0){
			return -1;
		}

		fclose(fpLOG);

		return 0;
	}
}
<commit_msg>test print<commit_after>#include "includes_defines.h"
#include "types.h"
#include "global.h"
#include "utilities.cpp"
#include "load_scenarios.cpp"
#include "heuristic_function.cpp"
#include "custom_encoding.cpp"
#include "encoding_methods.cpp"

extern "C" {

	int get_bit(int row, int col){
		return ((int) opcodes[row][col]);
	}

	int get_codes_length(){
		return bits;
	}

	int load_graphs_codes(char *file_in,
			char *custom_file_name){

		FILE *fp;
		int trivial = 0;
		int err=0;
		int elements;
		int min_disp;
printf("1-\n");
		if(first){
			first = FALSE;
			if(temporary_files_creation() != 0){
				return -1;
			}
		}
printf("2-\n");
		if( (fpLOG = fopen(LOG,"w")) == NULL){
			fprintf(stderr,"Error on opening LOG file for writing.\n");
		}
		fprintf(fpLOG,WELCOME_STRING);
printf("3-\n");
		// memory allocation
		fprintf(fpLOG,"Allocating memory for vertex names and graphs...");
		g = (GRAPH_TYPE *) malloc(sizeof(GRAPH_TYPE) * scenariosLimit);
		fprintf(fpLOG,"DONE\n");
		fflush(stdout);
printf("4-\n");
		//**********************************************************************
		// Building CPOG Part
		//**********************************************************************

		// loading scenarios
		fprintf(fpLOG,"\nOptimal scenarios encoding and CPOG synthesis.\n");
		if(loadScenarios(file_in, fp) != 0){
			fprintf(stderr,"Loading scenarios failed.\n");
			return -1;
		}
		fprintf(fpLOG,"\n%d scenarios have been loaded.\n", n);
printf("5-\n");
		// looking for predicates
		if(predicateSearch() != 0){
			fprintf(stderr,"Predicate searching failed.\n");
			return -1;
		}
printf("6-\n");
		// looking for non-trivial constraints
		if( (fp = fopen(CONSTRAINTS_FILE,"w")) == NULL){
			fprintf(stderr,"Error on opening constraints file for writing.\n");
			return -1;
		}
		if(nonTrivialConstraints(fp, &total, &trivial) != 0){
			fprintf(stderr,"Non-trivial constraints searching failed.\n");
			return -1;
		}
		fprintf(fpLOG,"\n%d non-trivial encoding constraints found:\n\n", total - trivial);
printf("7-\n");
		// writing non-trivial constraints into a file
		if( (fp = fopen(TRIVIAL_ENCODING_FILE,"w")) == NULL){
			fprintf(stderr,"Error on opening constraints file for writing.\n");
			return -1;
		}
		for(int i = 0; i < total; i++)
			if (!encodings[i].trivial) {
				fprintf(fp,"%s\n",encodings[i].constraint.c_str());
		}
		fclose(fp);
	printf("8-\n");
		fprintf(fpLOG,"\nBuilding conflict graph... ");
		if(conflictGraph(&total) != 0){
			fprintf(stderr,"Building conflict graph failed.\n");
			return -1;
		}
		fprintf(fpLOG,"DONE.\n");
printf("8-\n");
		//**********************************************************************
		// Reading encoding set by the user
		//**********************************************************************

		fprintf(fpLOG,"Reading encodings set... ");
		fflush(stdout);
		if(read_set_encoding(custom_file_name,n,&bits) != 0){
			fprintf(stderr,"Error on reading encoding set.\n");
			removeTempFiles();
			return -1;
		}
		fprintf(fpLOG,"DONE\n");
printf("9-\n");
		fprintf(fpLOG,"Check correcteness of encoding set... ");
		if(check_correctness(custom_file_name,n,tot_enc,bits) != 0){
			removeTempFiles();
			return -1;
		}
		fprintf(fpLOG,"DONE\n");
printf("10-\n");
		//**********************************************************************
		// Variable preparation for encoding
		//**********************************************************************
		strcpy(file_in,TRIVIAL_ENCODING_FILE);
		file_cons = strdup(CONSTRAINTS_FILE);

		/*READ NON-TRIVIAL ENCODING FILE*/
		fprintf(fpLOG,"Reading non-trivial encoding file... ");
		if( (err = read_file(file_in)) ){
			fprintf(stderr,"Error occured while reading non-trivial encoding file, error code: %d", err);
			removeTempFiles();
			return -1;
		}
		fprintf(fpLOG,"DONE\n");
printf("11-\n");
		/*SEED FOR RAND*/
		srand(time(NULL));
printf("12-\n");
		/*ALLOCATING AND ZEROING DIFFERENCE MATRIX*/
		opt_diff = (int**) calloc(n, sizeof(int*));
		for(int i=0;i<n;i++)
			opt_diff[i] = (int*) calloc(n, sizeof(int));
printf("13-\n");
		/*NUMBER OF POSSIBLE ENCODING*/
		tot_enc = 1;
		for(int i=0;i<bits;i++) tot_enc *= 2;
printf("14-\n");
		/*ANALYSIS IF IT'S A PERMUTATION OR A DISPOSITION*/
		num_perm = 1;
		if (n == tot_enc){
			/*PERMUTATION*/
			if(!unfix && !SET){
				for(int i = 1; i< tot_enc; i++)
					num_perm *= i;
			}else{
				for(int i = 1; i<= tot_enc; i++)
					num_perm *= i;
			}
			fprintf(fpLOG,"Number of possible permutations by fixing first element: %lld\n", num_perm);
		}
		else{
			/*DISPOSITION*/
			if(!unfix && !SET){
				elements = tot_enc-1;
				min_disp = elements - (n- 1) + 1;
			}else{
				elements = tot_enc;
				min_disp = elements - (n) + 1;
			}
				num_perm = 1;
			for(int i=elements; i>= min_disp; i--)
				num_perm *= i;
			fprintf(fpLOG,"Number of possible dispositions by fixing first element: %lld\n", num_perm);
		}
printf("15-\n");
		all_perm = num_perm;
//		num_perm = EXHAUSTIVE_MAX_PERM;

		/*PREPARATION DATA FOR ENCODING PERMUTATIONS*/
		enc = (int*) calloc(tot_enc, sizeof(int));

		/*First element is fixed*/
		if (!unfix && !SET)
			enc[0] = 1;

		sol = (int*) calloc(tot_enc, sizeof(int));
		if (sol == NULL){
			fprintf(stderr,"solution variable = null\n");
			removeTempFiles();
			return -1;
		}
printf("16-\n");
		/*BUILDING DIFFERENCE MATRIX*/
		fprintf(fpLOG,"Building DM (=Difference Matrix)... ");
		if( (err = difference_matrix(cpog_count)) ){
			fprintf(stderr,"Error occurred while building difference matrix, error code: %d", err);
			removeTempFiles();
			return -1;
		}
		fprintf(fpLOG,"DONE\n");
printf("17-\n");
		fclose(fpLOG);
		removeTempFiles();

		return 0;
	}

	int unload_graphs_codes(){
		if(g != NULL) free(g);
		if(manual_file != NULL) free(manual_file);
		if(manual_file_back != NULL) free(manual_file_back);
		if(custom_perm != NULL) free(custom_perm);
		if(custom_perm_back != NULL) free(custom_perm_back);
		if(opt_diff != NULL) {
			for(int i = 0; i<cpog_count; i++)
				if(opt_diff[i] != NULL) free(opt_diff[i]);
			free(opt_diff);
		}
		if(perm != NULL) {
			for(long long int i = 0; i<num_perm; i++)
				if(perm[i] != NULL )free(perm[i]);
			free(perm);
		}
		if(file_cons != NULL) free(file_cons);
		if(weights != NULL) free(weights);
		if(diff != NULL){
			for(int i = 0; i< len_sequence; i++)
				if(diff[i] != NULL) free(diff[i]);
			free(diff);
		}
		if(opcodes != NULL){
			for(int i = 0; i < cpog_count; i++)
				if(opcodes[i] != NULL) free(opcodes[i]);
		}
		if(enc != NULL) free(enc);
		if(sol != NULL) free(sol);
		cpog_count = 0;
		n = 0;
		len_sequence = 0;

		// Andrey's tool
		if( !eventNames.empty() ) eventNames.clear();
		for(int i = 0; i<eventsLimit; i++)
			if( !eventNames_str[i].empty() )eventNames_str[i].clear();
		for(int i = 0; i<eventsLimit;i++)
			if( !eventPredicates[i].empty() )
				eventPredicates[i].clear();
		if( !scenarioNames.empty() )scenarioNames.clear();
		if( !scenarioOpcodes.empty() )scenarioOpcodes.clear();
		for(int i = 0; i< eventsLimit; i++)
			for(int j = 0; j< predicatesLimit; j++)
				if(!ev[i][j].empty()) ev[i][j].clear();
		for(int i = 0; i< eventsLimit; i++)
			for(int j = 0; j< eventsLimit; j++)
				if(!ee[i][j].empty()) ee[i][j].clear();

		if(!constraints.empty()) constraints.clear();
		if ( !encodings.empty() )encodings.clear();
		if ( !cgv.empty() ) cgv.clear();
		if ( !cge.empty() ) cge.clear();
		if ( !literal.empty() ) literal.clear();
		if ( !bestLiteral.empty() ) bestLiteral.clear();

		for(int i = 0; i< eventsLimit; i++)
			for(int j = 0; j< predicatesLimit; j++)
				if(!vConditions[i][j].empty())
					vConditions[i][j].clear();

		for(int i = 0; i< eventsLimit; i++)
			for(int j = 0; j< eventsLimit; j++)
				if(!aConditions[i][j].empty())
					aConditions[i][j].clear();
		return 0;
	}

	int single_literal_encoding(){

		if( (fpLOG = fopen(LOG,"a")) == NULL){
			fprintf(stderr,"Error on opening LOG file for appending.\n");
		}

		fprintf(fpLOG,"Running single-literal encoding.\n");

		if(singleLiteralEncoding(total) != 0){
			fprintf(stderr,"Single-literal encoding failed.\n");
			return -1;
		}

		if (export_variables(single_literal) != 0){
			return -1;
		}

		fclose(fpLOG);

		return 0;
	}

	int sequential_encoding(){

		if( (fpLOG = fopen(LOG,"a")) == NULL){
			fprintf(stderr,"Error on opening LOG file for appending.\n");
		}

		fprintf(fpLOG,"Running Sequential encoding.\n");

		if(allocate_encodings_space(1) != 0){
			fprintf(stderr,"Sequential encoding failed.\n");
			return -1;
		}

		if(sequentialEncoding() != 0){
			fprintf(stderr,"Sequential encoding failed.\n");
			return -1;
		}

		if (export_variables(sequential) != 0){
			return -1;
		}

		fclose(fpLOG);

		return 0;
	}

	int random_encoding(int num_enc){

		if( (fpLOG = fopen(LOG,"a")) == NULL){
			fprintf(stderr,"Error on opening LOG file for appending.\n");
		}

		bits = bits_saved;

		fprintf(fpLOG,"Running Random encoding.\n");

		if(allocate_encodings_space(num_enc) != 0){
			fprintf(stderr,"Encoding allocation failed.\n");
			return -1;
		}

		if(randomEncoding() != 0){
			fprintf(stderr,"Random encoding failed.\n");
			return -1;
		}

		if(heuristic_choice() != 0) {
			fprintf(stderr,"Computing heuristic encoding.\n");
			return -1;
		}

		if (export_variables(randomE) != 0){
			return -1;
		}

		fclose(fpLOG);

		return 0;
	}

	int heuristic_encoding(int num_enc){

		if( (fpLOG = fopen(LOG,"a")) == NULL){
			fprintf(stderr,"Error on opening LOG file for appending.\n");
		}

		bits = bits_saved;

		if(allocate_encodings_space(num_enc) != 0){
			fprintf(stderr,"Encoding allocation failed.\n");
			return -1;
		}

		fprintf(fpLOG,"Running Random generation... ");

		if(randomEncoding() != 0){
			fprintf(stderr,"Random encoding failed.\n");
			return -1;
		}
		fprintf(fpLOG,"DONE.\n");

		fprintf(fpLOG,"Running Heuristic optimisation... ");
		if(start_simulated_annealing() != 0){
			fprintf(stderr,"Heuristic optimisation failed.\n");
			return -1;
		}
		fprintf(fpLOG,"DONE.\n");

		if(heuristic_choice() != 0) {
			fprintf(stderr,"Computing heuristic encoding.\n");
			return -1;
		}

		if (export_variables(heuristic) != 0){
			return -1;
		}

		fclose(fpLOG);

		return 0;
	}

	int exhaustive_encoding(int num_enc){

		if( (fpLOG = fopen(LOG,"a")) == NULL){
			fprintf(stderr,"Error on opening LOG file for appending.\n");
		}

		if(allocate_encodings_space(num_enc) != 0){
			fprintf(stderr,"Encoding allocation failed.\n");
			return -1;
		}

		fprintf(fpLOG,"Running Exhaustive encoding.\n");

		if(!unfix && !SET){
			//permutation_stdalgo(cpog_count,tot_enc);
			fprintf(fpLOG,"Permutation algorithm unconstrained... ");
			exhaustiveEncoding(sol,0,enc,cpog_count, tot_enc);
			fprintf(fpLOG,"DONE\n");
		}else{
			fprintf(fpLOG,"Permutation algorithm constrained... ");
			exhaustiveEncoding(sol,-1,enc,cpog_count, tot_enc);
			fprintf(fpLOG,"DONE\n");

			fprintf(fpLOG,"Filtering encoding... ");
			filter_encodings(cpog_count, bits, tot_enc);
			fprintf(fpLOG,"DONE\n");

		}

		if(heuristic_choice() != 0) {
			fprintf(stderr,"Computing heuristic encoding.\n");
			return -1;
		}

		if (export_variables(exhaustive) != 0){
			return -1;
		}

		fclose(fpLOG);

		return 0;
	}
}
<|endoftext|>


<commit_before>/*-----------------------------------------------------------------------------------------------
The MIT License (MIT)

Copyright (c) 2015 OSRE ( Open Source Render Engine ) by Kim Kulling

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
-----------------------------------------------------------------------------------------------*/
#include "OGLShader.h"

#include <osre/Common/Logger.h>
#include <osre/IO/Stream.h>

namespace OSRE {
namespace RenderBackend {

static const GLint ErrorId = -1;
static const String Tag    = "OGLShader";

static GLuint getOGLShaderType( ShaderType type ) {
    switch( type ) {
        case SH_VertexShaderType:
            return GL_VERTEX_SHADER;
        case SH_FragmentShaderType:
            return GL_FRAGMENT_SHADER;
        case SH_GeometryShaderType:
            return GL_GEOMETRY_SHADER;
        default:
           break;
    }

    return 0;
}

OGLShader::OGLShader( const String &name )
: Object( name )
, m_shaderprog( 0 )
, m_numShader( 0 )
, m_attributeList()
, m_uniformLocationList() {
    ::memset( m_shaders, 0, sizeof( unsigned int ) * 3 );
}

OGLShader::~OGLShader( ) {
    for ( unsigned int i=0; i<3; ++i ) {
        if( m_shaders[ i ] ) {
            glDeleteShader( m_shaders[ i ] );
            m_shaders[ i ] = 0;
        }
    }
}

bool OGLShader::loadFromSource( ShaderType type, const String &src ) {
    if ( src.empty() ) {
        return false;
    }
    GLuint shader = glCreateShader( getOGLShaderType( type ) );
    m_shaders[ type ] = shader;

    const char *tmp = src.c_str();
    glShaderSource( shader, 1, &tmp, nullptr );

    return true;
}

bool OGLShader::loadFromFile(ShaderType type, IO::Stream &stream ) {
    if ( !stream.isOpen() ) {
        return false;
    }

    ui32 filesize(stream.getSize());
    if ( 0 == filesize ) {
        return true;
    }

    c8 *data = new c8[ filesize ];
    stream.read( data, filesize);

    const bool retCode( loadFromSource( type, String( data ) ) );
    delete [] data;

    return retCode;
}

bool OGLShader::createAndLink() {
    m_shaderprog = glCreateProgram();
    if ( 0 == m_shaderprog ) {
        osre_error( Tag, "Error while creating shader program." );
        return false;
    }
    if ( 0 != m_shaders[ SH_VertexShaderType ] ) {
        glAttachShader( m_shaderprog, m_shaders[ SH_VertexShaderType ] );
    }
    if ( 0 != m_shaders[ SH_FragmentShaderType ] ) {
        glAttachShader( m_shaderprog, m_shaders[ SH_FragmentShaderType ] );
    }
    if ( 0 != m_shaders[ SH_GeometryShaderType ] ) {
        glAttachShader( m_shaderprog, m_shaders[ SH_GeometryShaderType ] );
    }

    bool result( true );
    GLint status( 0 );
    glLinkProgram( m_shaderprog );
    glGetProgramiv( m_shaderprog, GL_LINK_STATUS, &status );
    if (status == GL_FALSE) {
        GLint infoLogLength( 0 );
        glGetProgramiv( m_shaderprog, GL_INFO_LOG_LENGTH, &infoLogLength);
        GLchar *infoLog= new GLchar[infoLogLength];
        ::memset( infoLog, 0, infoLogLength );
        glGetProgramInfoLog( m_shaderprog, infoLogLength, NULL, infoLog);
        String error( infoLog );
        osre_debug( Tag, "Link log: " + error + "\n" );
        delete [] infoLog;
        result = false;
    }

    return result;
}

void OGLShader::use( ) {
    glUseProgram( m_shaderprog );
}

void OGLShader::unuse( ) {
    glUseProgram( 0 );
}

void OGLShader::addAttribute( const std::string& attribute ) {
    const GLint location = glGetAttribLocation( m_shaderprog, attribute.c_str( ) );
    m_attributeList[ attribute ] = location;
    if( ErrorId == location ) {
        osre_debug( Tag, "Cannot find attribute " + attribute + " in shader." );
    }
}

void OGLShader::addUniform( const std::string& uniform ) {
    const GLint location = glGetUniformLocation( m_shaderprog, uniform.c_str( ) );
    m_uniformLocationList[ uniform ] = location;
    if( ErrorId == location ) {
        osre_debug( Tag, "Cannot find uniform variable " + uniform + " in shader." );
    }
}

GLint OGLShader::operator[] ( const std::string &attribute ) {
    const GLint loc( m_attributeList[ attribute ] );
    return loc;
}

GLint OGLShader::operator() ( const std::string &uniform ) {
    const GLint loc( m_uniformLocationList[ uniform ] );
    return loc;
}

} // Namespace RenderBackend
} // Namespace OSRE
<commit_msg>Shader: reformat.<commit_after>/*-----------------------------------------------------------------------------------------------
The MIT License (MIT)

Copyright (c) 2015 OSRE ( Open Source Render Engine ) by Kim Kulling

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
-----------------------------------------------------------------------------------------------*/
#include "OGLShader.h"

#include <osre/Common/Logger.h>
#include <osre/IO/Stream.h>

namespace OSRE {
namespace RenderBackend {

static const GLint ErrorId = -1;
static const String Tag    = "OGLShader";

static GLuint getOGLShaderType( ShaderType type ) {
    switch( type ) {
        case SH_VertexShaderType:
            return GL_VERTEX_SHADER;
        case SH_FragmentShaderType:
            return GL_FRAGMENT_SHADER;
        case SH_GeometryShaderType:
            return GL_GEOMETRY_SHADER;
        default:
           break;
    }

    return 0;
}

OGLShader::OGLShader( const String &name )
: Object( name )
, m_shaderprog( 0 )
, m_numShader( 0 )
, m_attributeList()
, m_uniformLocationList() {
    ::memset( m_shaders, 0, sizeof( unsigned int ) * 3 );
}

OGLShader::~OGLShader( ) {
    for ( unsigned int i=0; i<3; ++i ) {
        if( m_shaders[ i ] ) {
            glDeleteShader( m_shaders[ i ] );
            m_shaders[ i ] = 0;
        }
    }
}

bool OGLShader::loadFromSource( ShaderType type, const String &src ) {
    if ( src.empty() ) {
        return false;
    }
    GLuint shader = glCreateShader( getOGLShaderType( type ) );
    m_shaders[ type ] = shader;

    const char *tmp = src.c_str();
    glShaderSource( shader, 1, &tmp, nullptr );

    return true;
}

bool OGLShader::loadFromFile( ShaderType type, IO::Stream &stream ) {
    if ( !stream.isOpen() ) {
        return false;
    }

    ui32 filesize(stream.getSize());
    if ( 0 == filesize ) {
        return true;
    }

    c8 *data = new c8[ filesize ];
    stream.read( data, filesize);

    const bool retCode( loadFromSource( type, String( data ) ) );
    delete [] data;

    return retCode;
}

bool OGLShader::createAndLink() {
    m_shaderprog = glCreateProgram();
    if ( 0 == m_shaderprog ) {
        osre_error( Tag, "Error while creating shader program." );
        return false;
    }
    if ( 0 != m_shaders[ SH_VertexShaderType ] ) {
        glAttachShader( m_shaderprog, m_shaders[ SH_VertexShaderType ] );
    }
    if ( 0 != m_shaders[ SH_FragmentShaderType ] ) {
        glAttachShader( m_shaderprog, m_shaders[ SH_FragmentShaderType ] );
    }
    if ( 0 != m_shaders[ SH_GeometryShaderType ] ) {
        glAttachShader( m_shaderprog, m_shaders[ SH_GeometryShaderType ] );
    }

    bool result( true );
    GLint status( 0 );
    glLinkProgram( m_shaderprog );
    glGetProgramiv( m_shaderprog, GL_LINK_STATUS, &status );
    if (status == GL_FALSE) {
        GLint infoLogLength( 0 );
        glGetProgramiv( m_shaderprog, GL_INFO_LOG_LENGTH, &infoLogLength);
        GLchar *infoLog= new GLchar[infoLogLength];
        ::memset( infoLog, 0, infoLogLength );
        glGetProgramInfoLog( m_shaderprog, infoLogLength, NULL, infoLog);
        String error( infoLog );
        osre_debug( Tag, "Link log: " + error + "\n" );
        delete [] infoLog;
        result = false;
    }

    return result;
}

void OGLShader::use( ) {
    glUseProgram( m_shaderprog );
}

void OGLShader::unuse( ) {
    glUseProgram( 0 );
}

void OGLShader::addAttribute( const std::string& attribute ) {
    const GLint location = glGetAttribLocation( m_shaderprog, attribute.c_str( ) );
    m_attributeList[ attribute ] = location;
    if( ErrorId == location ) {
        osre_debug( Tag, "Cannot find attribute " + attribute + " in shader." );
    }
}

void OGLShader::addUniform( const std::string& uniform ) {
    const GLint location = glGetUniformLocation( m_shaderprog, uniform.c_str( ) );
    m_uniformLocationList[ uniform ] = location;
    if( ErrorId == location ) {
        osre_debug( Tag, "Cannot find uniform variable " + uniform + " in shader." );
    }
}

GLint OGLShader::operator[] ( const std::string &attribute ) {
    const GLint loc( m_attributeList[ attribute ] );
    return loc;
}

GLint OGLShader::operator() ( const std::string &uniform ) {
    const GLint loc( m_uniformLocationList[ uniform ] );
    return loc;
}

} // Namespace RenderBackend
} // Namespace OSRE
<|endoftext|>


<commit_before>/**
 * @file
 *
 * @brief Tests for the getenv library
 *
 * @copyright BSD License (see doc/COPYING or http://www.libelektra.org)
 *
 */

#include <gtest/gtest.h>
#include <kdbgetenv.h>

TEST (GetEnv, NonExist)
{
	EXPECT_EQ (getenv ("du4Maiwi/does-not-exist"), static_cast<char *> (nullptr));
}

TEST (GetEnv, ExistOverride)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	ksAppendKey (elektraConfig, keyNew ("user/elektra/intercept/getenv/override/does-exist", KEY_VALUE, "hello", KEY_END));
	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ExistOverrideFallback)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	ksAppendKey (elektraConfig, keyNew ("user/env/override/does-exist-fb", KEY_VALUE, "hello", KEY_END));
	ASSERT_NE (getenv ("does-exist-fb"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist-fb"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ExistEnv)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	setenv ("does-exist", "hello", 1);
	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ExistEnvFallback)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	setenv ("does-exist-fb", "hello", 1);
	ASSERT_NE (getenv ("does-exist-fb"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist-fb"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ExistFallback)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	ksAppendKey (elektraConfig, keyNew ("user/elektra/intercept/getenv/fallback/does-exist", KEY_VALUE, "hello", KEY_END));
	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ExistFallbackFallback)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	ksAppendKey (elektraConfig, keyNew ("user/elektra/intercept/getenv/fallback/does-exist-fb", KEY_VALUE, "hello", KEY_END));
	ASSERT_NE (getenv ("does-exist-fb"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist-fb"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, OpenClose)
{
	using namespace ckdb;
	// KeySet *oldElektraConfig = elektraConfig;
	elektraOpen (nullptr, nullptr);
	// EXPECT_NE(elektraConfig, oldElektraConfig); // even its a new object, it might point to same address
	EXPECT_EQ (getenv ("du4Maiwi/does-not-exist"), static_cast<char *> (nullptr));
	ksAppendKey (elektraConfig, keyNew ("user/elektra/intercept/getenv/override/does-exist", KEY_VALUE, "hello", KEY_END));

	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	EXPECT_EQ (getenv ("du4Maiwi/does-not-exist"), static_cast<char *> (nullptr));
	elektraClose ();
}

TEST (GetEnv, OpenCloseFallback)
{
	using namespace ckdb;
	// KeySet *oldElektraConfig = elektraConfig;
	elektraOpen (nullptr, nullptr);
	// EXPECT_NE(elektraConfig, oldElektraConfig); // even its a new object, it might point to same address
	EXPECT_EQ (getenv ("du4Maiwi/does-not-exist-fb"), static_cast<char *> (nullptr));
	ksAppendKey (elektraConfig, keyNew ("user/env/override/does-exist-fb", KEY_VALUE, "hello", KEY_END));

	ASSERT_NE (getenv ("does-exist-fb"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist-fb"), std::string ("hello"));
	EXPECT_EQ (getenv ("du4Maiwi/does-not-exist-fb"), static_cast<char *> (nullptr));
	elektraClose ();
}

void elektraPrintConfig ()
{
	using namespace ckdb;
	Key * c;
	ksRewind (elektraConfig);
	while ((c = ksNext (elektraConfig)))
	{
		printf ("%s - %s\n", keyName (c), keyString (c));
	}
}

TEST (GetEnv, ArgvParam)
{
	const char * cargv[] = { "name", "--elektra:does-exist=hello", nullptr };
	char ** argv = const_cast<char **> (cargv);
	int argc = 2;
	using namespace ckdb;
	elektraOpen (&argc, argv);
	EXPECT_EQ (argc, 1) << "elektra proc not consumed";
	EXPECT_EQ (argv[0], std::string ("name"));
	EXPECT_EQ (argv[1], static_cast<char *> (nullptr));

	ckdb::Key * k = ksLookupByName (elektraConfig, "proc/elektra/intercept/getenv/override/does-exist", 0);

	ASSERT_NE (k, static_cast<ckdb::Key *> (nullptr));
	EXPECT_EQ (keyString (k), std::string ("hello"));

	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ArgvParamUninvolved)
{
	const char * cargv[] = { "name",	 "--uninvolved", "--not-used", "-L",   "--elektra:does-exist=hello",
				 "--uninvolved", "--not-used",   "-L",	 nullptr };
	char ** argv = const_cast<char **> (cargv);
	int argc = 8;
	using namespace ckdb;
	elektraOpen (&argc, argv);
	EXPECT_EQ (argc, 7) << "elektra proc not consumed";
	EXPECT_EQ (argv[0], std::string ("name"));
	EXPECT_EQ (argv[1], std::string ("--uninvolved"));
	EXPECT_EQ (argv[2], std::string ("--not-used"));
	EXPECT_EQ (argv[3], std::string ("-L"));
	EXPECT_EQ (argv[4], std::string ("--uninvolved"));
	EXPECT_EQ (argv[5], std::string ("--not-used"));
	EXPECT_EQ (argv[6], std::string ("-L"));
	EXPECT_EQ (argv[7], static_cast<char *> (nullptr));

	ckdb::Key * k = ksLookupByName (elektraConfig, "proc/elektra/intercept/getenv/override/does-exist", 0);

	ASSERT_NE (k, static_cast<ckdb::Key *> (nullptr));
	EXPECT_EQ (keyString (k), std::string ("hello"));

	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, NameArgv0)
{
	using namespace ckdb;
	int argc = 1;
	const char * cargv[] = { "path/to/any-name", nullptr };
	char ** argv = const_cast<char **> (cargv);

	elektraOpen (&argc, argv);
	ckdb::Key * k = ksLookupByName (elektraConfig, "proc/elektra/intercept/getenv/layer/name", 0);
	ASSERT_NE (k, static_cast<ckdb::Key *> (nullptr));
	EXPECT_EQ (keyString (k), std::string ("path/to/any-name"));

	k = ksLookupByName (elektraConfig, "proc/elektra/intercept/getenv/layer/basename", 0);
	ASSERT_NE (k, static_cast<ckdb::Key *> (nullptr));
	EXPECT_EQ (keyString (k), std::string ("any-name"));
	elektraClose ();
}

TEST (GetEnv, NameExplicit)
{
	using namespace ckdb;
	int argc = 2;
	const char * cargv[] = { "any-name", "--elektra%name%=other-name" };
	char ** argv = const_cast<char **> (cargv);

	elektraOpen (&argc, argv);
	ckdb::Key * k = ksLookupByName (elektraConfig, "proc/elektra/intercept/getenv/layer/name", 0);
	ASSERT_NE (k, static_cast<ckdb::Key *> (nullptr));
	EXPECT_EQ (keyString (k), std::string ("other-name"));
	elektraClose ();
}

#include "main.cpp"
<commit_msg>test_getenv: fixed testcase<commit_after>/**
 * @file
 *
 * @brief Tests for the getenv library
 *
 * @copyright BSD License (see doc/COPYING or http://www.libelektra.org)
 *
 */

#include <gtest/gtest.h>
#include <kdbgetenv.h>

TEST (GetEnv, NonExist)
{
	EXPECT_EQ (getenv ("du4Maiwi/does-not-exist"), static_cast<char *> (nullptr));
}

TEST (GetEnv, ExistOverride)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	ksAppendKey (elektraConfig, keyNew ("user/elektra/intercept/getenv/override/does-exist", KEY_VALUE, "hello", KEY_END));
	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ExistOverrideFallback)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	ksAppendKey (elektraConfig, keyNew ("user/env/override/does-exist-fb", KEY_VALUE, "hello", KEY_END));
	ASSERT_NE (getenv ("does-exist-fb"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist-fb"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ExistEnv)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	setenv ("does-exist", "hello", 1);
	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ExistEnvFallback)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	setenv ("does-exist-fb", "hello", 1);
	ASSERT_NE (getenv ("does-exist-fb"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist-fb"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ExistFallback)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	ksAppendKey (elektraConfig, keyNew ("user/elektra/intercept/getenv/fallback/does-exist", KEY_VALUE, "hello", KEY_END));
	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ExistFallbackFallback)
{
	using namespace ckdb;
	elektraOpen (nullptr, nullptr);
	ksAppendKey (elektraConfig, keyNew ("user/env/fallback/does-exist-fb", KEY_VALUE, "hello", KEY_END));
	ASSERT_NE (getenv ("does-exist-fb"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist-fb"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, OpenClose)
{
	using namespace ckdb;
	// KeySet *oldElektraConfig = elektraConfig;
	elektraOpen (nullptr, nullptr);
	// EXPECT_NE(elektraConfig, oldElektraConfig); // even its a new object, it might point to same address
	EXPECT_EQ (getenv ("du4Maiwi/does-not-exist"), static_cast<char *> (nullptr));
	ksAppendKey (elektraConfig, keyNew ("user/elektra/intercept/getenv/override/does-exist", KEY_VALUE, "hello", KEY_END));

	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	EXPECT_EQ (getenv ("du4Maiwi/does-not-exist"), static_cast<char *> (nullptr));
	elektraClose ();
}

TEST (GetEnv, OpenCloseFallback)
{
	using namespace ckdb;
	// KeySet *oldElektraConfig = elektraConfig;
	elektraOpen (nullptr, nullptr);
	// EXPECT_NE(elektraConfig, oldElektraConfig); // even its a new object, it might point to same address
	EXPECT_EQ (getenv ("du4Maiwi/does-not-exist-fb"), static_cast<char *> (nullptr));
	ksAppendKey (elektraConfig, keyNew ("user/env/override/does-exist-fb", KEY_VALUE, "hello", KEY_END));

	ASSERT_NE (getenv ("does-exist-fb"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist-fb"), std::string ("hello"));
	EXPECT_EQ (getenv ("du4Maiwi/does-not-exist-fb"), static_cast<char *> (nullptr));
	elektraClose ();
}

void elektraPrintConfig ()
{
	using namespace ckdb;
	Key * c;
	ksRewind (elektraConfig);
	while ((c = ksNext (elektraConfig)))
	{
		printf ("%s - %s\n", keyName (c), keyString (c));
	}
}

TEST (GetEnv, ArgvParam)
{
	const char * cargv[] = { "name", "--elektra:does-exist=hello", nullptr };
	char ** argv = const_cast<char **> (cargv);
	int argc = 2;
	using namespace ckdb;
	elektraOpen (&argc, argv);
	EXPECT_EQ (argc, 1) << "elektra proc not consumed";
	EXPECT_EQ (argv[0], std::string ("name"));
	EXPECT_EQ (argv[1], static_cast<char *> (nullptr));

	ckdb::Key * k = ksLookupByName (elektraConfig, "proc/elektra/intercept/getenv/override/does-exist", 0);

	ASSERT_NE (k, static_cast<ckdb::Key *> (nullptr));
	EXPECT_EQ (keyString (k), std::string ("hello"));

	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, ArgvParamUninvolved)
{
	const char * cargv[] = { "name",	 "--uninvolved", "--not-used", "-L",   "--elektra:does-exist=hello",
				 "--uninvolved", "--not-used",   "-L",	 nullptr };
	char ** argv = const_cast<char **> (cargv);
	int argc = 8;
	using namespace ckdb;
	elektraOpen (&argc, argv);
	EXPECT_EQ (argc, 7) << "elektra proc not consumed";
	EXPECT_EQ (argv[0], std::string ("name"));
	EXPECT_EQ (argv[1], std::string ("--uninvolved"));
	EXPECT_EQ (argv[2], std::string ("--not-used"));
	EXPECT_EQ (argv[3], std::string ("-L"));
	EXPECT_EQ (argv[4], std::string ("--uninvolved"));
	EXPECT_EQ (argv[5], std::string ("--not-used"));
	EXPECT_EQ (argv[6], std::string ("-L"));
	EXPECT_EQ (argv[7], static_cast<char *> (nullptr));

	ckdb::Key * k = ksLookupByName (elektraConfig, "proc/elektra/intercept/getenv/override/does-exist", 0);

	ASSERT_NE (k, static_cast<ckdb::Key *> (nullptr));
	EXPECT_EQ (keyString (k), std::string ("hello"));

	ASSERT_NE (getenv ("does-exist"), static_cast<char *> (nullptr));
	EXPECT_EQ (getenv ("does-exist"), std::string ("hello"));
	elektraClose ();
}

TEST (GetEnv, NameArgv0)
{
	using namespace ckdb;
	int argc = 1;
	const char * cargv[] = { "path/to/any-name", nullptr };
	char ** argv = const_cast<char **> (cargv);

	elektraOpen (&argc, argv);
	ckdb::Key * k = ksLookupByName (elektraConfig, "proc/elektra/intercept/getenv/layer/name", 0);
	ASSERT_NE (k, static_cast<ckdb::Key *> (nullptr));
	EXPECT_EQ (keyString (k), std::string ("path/to/any-name"));

	k = ksLookupByName (elektraConfig, "proc/elektra/intercept/getenv/layer/basename", 0);
	ASSERT_NE (k, static_cast<ckdb::Key *> (nullptr));
	EXPECT_EQ (keyString (k), std::string ("any-name"));
	elektraClose ();
}

TEST (GetEnv, NameExplicit)
{
	using namespace ckdb;
	int argc = 2;
	const char * cargv[] = { "any-name", "--elektra%name%=other-name" };
	char ** argv = const_cast<char **> (cargv);

	elektraOpen (&argc, argv);
	ckdb::Key * k = ksLookupByName (elektraConfig, "proc/elektra/intercept/getenv/layer/name", 0);
	ASSERT_NE (k, static_cast<ckdb::Key *> (nullptr));
	EXPECT_EQ (keyString (k), std::string ("other-name"));
	elektraClose ();
}

#include "main.cpp"
<|endoftext|>


<commit_before>// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: Factory_test.C,v 1.3 2002/12/18 16:00:39 sturm Exp $
#include <BALL/CONCEPT/classTest.h>

///////////////////////////

#include <BALL/CONCEPT/factory.h>

///////////////////////////

START_TEST(Factory, "$Id: Factory_test.C,v 1.3 2002/12/18 16:00:39 sturm Exp $")

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////

using namespace BALL;

CHECK(Factory::getDefault())
	Size& def(const_cast<Size&>(Factory<Size>::getDefault()));
	def = 1234;
	TEST_EQUAL(Factory<Size>::getDefault(), def);
	def = 3456;
	TEST_EQUAL(Factory<Size>::getDefault(), def);
RESULT

CHECK(Factory::create())
	Size* ptr = Factory<Size>::create();
	TEST_NOT_EQUAL(ptr, 0)
	Size* ptr2 = Factory<Size>::create();
	TEST_NOT_EQUAL(ptr, 0)
	TEST_NOT_EQUAL(ptr, ptr2)
	delete ptr;
	delete ptr2;
RESULT

CHECK(Factory::createVoid())
	Size* ptr = (Size*)Factory<Size>::create();
	TEST_NOT_EQUAL(ptr, 0)
	Size* ptr2 = (Size*)Factory<Size>::create();
	TEST_NOT_EQUAL(ptr, 0)
	TEST_NOT_EQUAL(ptr, ptr2)
	delete ptr;
	delete ptr2;
RESULT

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
END_TEST
<commit_msg>fixed: CHECK headers<commit_after>// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: Factory_test.C,v 1.4 2003/06/16 15:43:24 anker Exp $
#include <BALL/CONCEPT/classTest.h>

///////////////////////////

#include <BALL/CONCEPT/factory.h>

///////////////////////////

START_TEST(Factory, "$Id: Factory_test.C,v 1.4 2003/06/16 15:43:24 anker Exp $")

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////

using namespace BALL;

CHECK(static const T& getDefault())
	Size& def(const_cast<Size&>(Factory<Size>::getDefault()));
	def = 1234;
	TEST_EQUAL(Factory<Size>::getDefault(), def);
	def = 3456;
	TEST_EQUAL(Factory<Size>::getDefault(), def);
RESULT

CHECK(static T* create())
	Size* ptr = Factory<Size>::create();
	TEST_NOT_EQUAL(ptr, 0)
	Size* ptr2 = Factory<Size>::create();
	TEST_NOT_EQUAL(ptr, 0)
	TEST_NOT_EQUAL(ptr, ptr2)
	delete ptr;
	delete ptr2;
RESULT

CHECK(static void* createVoid())
	Size* ptr = (Size*)Factory<Size>::create();
	TEST_NOT_EQUAL(ptr, 0)
	Size* ptr2 = (Size*)Factory<Size>::create();
	TEST_NOT_EQUAL(ptr, 0)
	TEST_NOT_EQUAL(ptr, ptr2)
	delete ptr;
	delete ptr2;
RESULT

/////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////
END_TEST
<|endoftext|>


<commit_before>#include "localization.h"

#include "gui_msgs/ColorMsg.h"
#include "gui_msgs/GUIPointMsg.h"
#include "gui_msgs/GUIPolyMsg.h"
#include "rss_msgs/MotionMsg.h"
#include "rss_msgs/RobotLocation.h"
#include <random>

using namespace rss_msgs;
using namespace gui_msgs;
using namespace std;

typedef cgal_kernel K;
typedef K::Point_2 Point_2;
typedef K::Point_3 Point_3;
typedef K::Ray_2 Ray_2;
typedef K::Vector_2 Vector_2;
typedef CGAL::Polygon_2<K> Polygon_2;

namespace localization {

double curTime() {
  return ros::Time::now().toSec();
}

Localization::Localization() {
  ROS_DEBUG("Initializing localization module");

  _time = curTime();

  // Initializing the sonar locations.
  SONAR_DIR = {
    cgal_kernel::Vector_2(0.0, 1.0),
    cgal_kernel::Vector_2(0.0, 1.0)
  };
  SONAR_POS = {
    cgal_kernel::Vector_2(-0.28, 0.215),
    cgal_kernel::Vector_2(0.025, 0.215)
  };

  ros::NodeHandle n;
  // Initialize message publishers.
  _location_pub = n.advertise<RobotLocation>("localization/update", 1000);
  _guipoly_pub = n.advertise<GUIPolyMsg>("gui/Poly", 1000);
  _guipoint_pub = n.advertise<GUIPointMsg>("gui/Point", 1000);

  // Load map file location and initialize the map-handler class instance.
  string mapfile_location;
  assert(n.getParam("/loc/mapFileName", mapfile_location));
  ROS_DEBUG("Got mapFileName: %s", mapfile_location.c_str());
  _wall_map = make_shared<WallMap>(mapfile_location);
  ROS_DEBUG("Initialized wall map.");

  InitializeParticles();

  // Test CASE 01
  {
    string res;
    if (n.getParam("/loc/testTriangulation", res)) {
      if (res == "yes") {
        TestTriangulation();
      }
    }
  }
  // Test CASE 02
  {
    string res;
    if (n.getParam("/loc/leaveBreadCrumbs", res)) {
      if (res == "yes") {
        _leaveBreadCrumbs = true;
      }
    }
  }
}

// Initialize with N random points generated within a given tolerance (in 3-D
// space, it's a parallelipiped with sides equal to tolerance values)
void Localization::InitializeParticles() {
  _particles.resize(N);

  double sx = CGAL::to_double(_wall_map->GetRobotStart().x());
  double dx = DISTANCE_TOLERANCE;
  double sy = CGAL::to_double(_wall_map->GetRobotStart().y());
  double dy = DISTANCE_TOLERANCE;
  double st = 0.0;
  double dt = HEADING_TOLERANCE;

  default_random_engine generator;
  uniform_real_distribution<double> xDist(sx-dx, sx+dx);
  uniform_real_distribution<double> yDist(sy-dy, sy+dy);
  uniform_real_distribution<double> tDist(st-dt, st+dt);

  for (int i = 0; i < N; ++i) {
    double x = xDist(generator);
    double y = yDist(generator);
    double t = tDist(generator);
    _particles[i] = Particle(x, y, t, -log(1.0*N));
  }
}

RobotLocation Localization::currentPositionBelief() const {
  double x = 0, y = 0, t = 0;
  double normalizer = 0.0;
  for (auto par : _particles) {
    double prob = exp(par.belief);
    x += par.x*prob;
    y += par.y*prob;
    par.t = NormalizeRad(par.t);
    t += par.t*prob;
    normalizer += prob;
  }

  x /= normalizer;
  y /= normalizer;
  t /= normalizer;
  t = NormalizeRad(t);

  RobotLocation currentPosition;
  currentPosition.x = x;
  currentPosition.y = y;
  currentPosition.theta = t;
  return currentPosition;
}

void Localization::PublishLocation() {
  RobotLocation currentBelief = currentPositionBelief();
  // TODO: remove
  currentBelief.x = _prev_odo_x;
  currentBelief.y = _prev_odo_y;
  currentBelief.theta = _prev_odo_t;
  _location_pub.publish(currentBelief);

  if (_leaveBreadCrumbs) {
    GUIPointMsg crumb;
    ColorMsg color;
    color.r = 0;
    color.g = 255;
    color.b = 0;
    crumb.color = color;
    crumb.x = currentBelief.x;
    crumb.y = currentBelief.y;
    _guipoint_pub.publish(crumb);
  }
}

void Localization::TestTriangulation() {
  // TEST
  ROS_DEBUG("Distance to wall from 0.0 0.0 in direction 1.0 0.0: %.3lf",
           _wall_map->DistanceToWall(K::Ray_2(Point_2(0.0,0.0), K::Direction_2(1.0,1.00000))));
  // Initialize distribution:
  ros::Duration(5.0).sleep(); // Sleep for 5 sec to allow gui node to initialize.
  for (const K::Triangle_2 &tri : _wall_map->_triangles) {
    GUIPolyMsg new_poly;
    new_poly.numVertices = 3;
    for (int i = 0; i < 3; ++i) {
      new_poly.x.push_back(CGAL::to_double(tri[i].x()));
      new_poly.y.push_back(CGAL::to_double(tri[i].y()));
    }
    new_poly.c.r = 255;
    new_poly.c.g = 0;
    new_poly.c.b = 0;
    new_poly.closed = 1;
    _guipoly_pub.publish(new_poly);
  }
}

double logGaussian(double x, double mu, double var) {
  return -0.5*log(2.0*M_PI*var) - (x-mu)*(x-mu)/var/2.0;
}
double logGaussian(double x, double y, double mux, double muy, double var) {
  return -0.5*log(2.0*M_PI*var) - ((x-mux)*(x-mux)+(y-muy)*(y-muy))/var/2.0;
}

void Localization::NormalizeBeliefs() {
  // Normalize the result
  double normalizer = 0.0;
  for (const Particle &par : _particles) {
    normalizer += exp(par.belief);
  }
  for (Particle &par : _particles) {
    par.belief -= log(normalizer);
  }
}

void Localization::onOdometryUpdate(const OdometryMsg::ConstPtr &odo) {
  double dx = odo->x-_prev_odo_x;
  double dy = odo->y-_prev_odo_y;
  double dt = odo->theta-_prev_odo_t;
  _prev_odo_x = odo->x;
  _prev_odo_y = odo->y;
  _prev_odo_t = odo->theta;
  _prev_odo_time = curTime();

  double start_time = curTime();
  ROS_DEBUG_STREAM("onOdometryUpdate: " << dx << " " << dy << " dt: " << dt << " at time: " << curTime()-_time);

  double varD = pow(min(0.03, 0.01 + sqrt(dx*dx+dy*dy)), 2.0);
  double varT = pow(min(0.17453292519, 0.17453292519/20.0+fabs(dt)), 2.0);

  default_random_engine gen;
  normal_distribution<double> xyDist(0.0, varD);
  normal_distribution<double> tDist(0.0, varT);

  vector<Particle> new_particles;
  for (const Particle &par : _particles) {
    new_particles.push_back(Particle(par.x+dx, par.y+dy, par.t+dt,
          par.belief + logGaussian(0.0, 0.0, 0.0, 0.0, varD) + logGaussian(0.0, 0.0, varT)));
    // Generate 10 random points to account for noise.
    for (int i = 0; i < 10; ++i) {
      double x = xyDist(gen) + par.x+dx;
      double y = xyDist(gen) + par.y+dy;
      double t = tDist(gen) + par.t+dt;
      new_particles.push_back(Particle(x, y, t,
            par.belief + logGaussian(x-par.x-dx, 0.0, y-par.y-dy, 0.0, varD) + logGaussian(t-par.t-dt, 0.0, varT)));
    }
  }

  ROS_DEBUG("onOdometryUpdate: time up to generation %.3lf sec.", curTime()-start_time);
  // Substitute old particles with top #N new ones.
  sort(new_particles.begin(), new_particles.end());
  reverse(new_particles.begin(), new_particles.end());
  new_particles.resize(N);
  _particles = new_particles;
  ROS_DEBUG("onOdometryUpdate: time up to sorting %.3lf sec.", curTime()-start_time);

  NormalizeBeliefs();
  PublishLocation();
  ROS_DEBUG("onOdometryUpdate: time passed %.3lf sec.", curTime()-start_time);
}

Vector_2 Rotate(Vector_2 vec, double alfa) {
  return Vector_2(vec.x()*cos(alfa) - vec.y()*sin(alfa),
                  vec.x()*sin(alfa) + vec.y()*cos(alfa));
}

void Localization::onSonarUpdate(const SonarMsg::ConstPtr &son) {
  ROS_DEBUG_STREAM("Got sonar update: " << son->sonarId << " range: " << son->range);
  return;
  double start_time = curTime();
  double varD = 0.01;
  for (Particle &par : _particles) {
    Vector_2 dir(Rotate(SONAR_DIR[son->sonarId], par.t));
    Point_2 sonarLoc(Point_2(par.x, par.y) + Rotate(SONAR_POS[son->sonarId], par.t));
    double mu = _wall_map->DistanceToWall(Ray_2(sonarLoc, dir));
    par.belief += logGaussian(son->range, mu, varD);
  }
  NormalizeBeliefs();
  PublishLocation();
  ROS_DEBUG("onSonarUpdate: time passed %.3lf sec.", curTime()-start_time);
}

double NormalizeRad(double rad) {
  rad = fmod(rad, 2*M_PI);
  if (rad < 0) {
    rad += 2*M_PI;
  }
  return rad;
}

} // namespace navigation
<commit_msg>Added new position of sonar<commit_after>#include "localization.h"

#include "gui_msgs/ColorMsg.h"
#include "gui_msgs/GUIPointMsg.h"
#include "gui_msgs/GUIPolyMsg.h"
#include "rss_msgs/MotionMsg.h"
#include "rss_msgs/RobotLocation.h"
#include <random>

using namespace rss_msgs;
using namespace gui_msgs;
using namespace std;

typedef cgal_kernel K;
typedef K::Point_2 Point_2;
typedef K::Point_3 Point_3;
typedef K::Ray_2 Ray_2;
typedef K::Vector_2 Vector_2;
typedef CGAL::Polygon_2<K> Polygon_2;

namespace localization {

double curTime() {
  return ros::Time::now().toSec();
}

Localization::Localization() {
  ROS_DEBUG("Initializing localization module");

  _time = curTime();

  // Initializing the sonar locations.
  SONAR_DIR = {
    cgal_kernel::Vector_2(0.0, -1.0),
    cgal_kernel::Vector_2(0.0, 1.0)
  };
  SONAR_POS = {
    cgal_kernel::Vector_2(-0.0975, -0.215),
    cgal_kernel::Vector_2(0.025, 0.215)
  };

  ros::NodeHandle n;
  // Initialize message publishers.
  _location_pub = n.advertise<RobotLocation>("localization/update", 1000);
  _guipoly_pub = n.advertise<GUIPolyMsg>("gui/Poly", 1000);
  _guipoint_pub = n.advertise<GUIPointMsg>("gui/Point", 1000);

  // Load map file location and initialize the map-handler class instance.
  string mapfile_location;
  assert(n.getParam("/loc/mapFileName", mapfile_location));
  ROS_DEBUG("Got mapFileName: %s", mapfile_location.c_str());
  _wall_map = make_shared<WallMap>(mapfile_location);
  ROS_DEBUG("Initialized wall map.");

  InitializeParticles();

  // Test CASE 01
  {
    string res;
    if (n.getParam("/loc/testTriangulation", res)) {
      if (res == "yes") {
        TestTriangulation();
      }
    }
  }
  // Test CASE 02
  {
    string res;
    if (n.getParam("/loc/leaveBreadCrumbs", res)) {
      if (res == "yes") {
        _leaveBreadCrumbs = true;
      }
    }
  }
}

// Initialize with N random points generated within a given tolerance (in 3-D
// space, it's a parallelipiped with sides equal to tolerance values)
void Localization::InitializeParticles() {
  _particles.resize(N);

  double sx = CGAL::to_double(_wall_map->GetRobotStart().x());
  double dx = DISTANCE_TOLERANCE;
  double sy = CGAL::to_double(_wall_map->GetRobotStart().y());
  double dy = DISTANCE_TOLERANCE;
  double st = 0.0;
  double dt = HEADING_TOLERANCE;

  default_random_engine generator;
  uniform_real_distribution<double> xDist(sx-dx, sx+dx);
  uniform_real_distribution<double> yDist(sy-dy, sy+dy);
  uniform_real_distribution<double> tDist(st-dt, st+dt);

  for (int i = 0; i < N; ++i) {
    double x = xDist(generator);
    double y = yDist(generator);
    double t = tDist(generator);
    _particles[i] = Particle(x, y, t, -log(1.0*N));
  }
}

RobotLocation Localization::currentPositionBelief() const {
  double x = 0, y = 0, t = 0;
  double normalizer = 0.0;
  for (auto par : _particles) {
    double prob = exp(par.belief);
    x += par.x*prob;
    y += par.y*prob;
    par.t = NormalizeRad(par.t);
    t += par.t*prob;
    normalizer += prob;
  }

  x /= normalizer;
  y /= normalizer;
  t /= normalizer;
  t = NormalizeRad(t);

  RobotLocation currentPosition;
  currentPosition.x = x;
  currentPosition.y = y;
  currentPosition.theta = t;
  return currentPosition;
}

void Localization::PublishLocation() {
  RobotLocation currentBelief = currentPositionBelief();
  // TODO: remove
  currentBelief.x = _prev_odo_x;
  currentBelief.y = _prev_odo_y;
  currentBelief.theta = _prev_odo_t;
  _location_pub.publish(currentBelief);

  if (_leaveBreadCrumbs) {
    GUIPointMsg crumb;
    ColorMsg color;
    color.r = 0;
    color.g = 255;
    color.b = 0;
    crumb.color = color;
    crumb.x = currentBelief.x;
    crumb.y = currentBelief.y;
    _guipoint_pub.publish(crumb);
  }
}

void Localization::TestTriangulation() {
  // TEST
  ROS_DEBUG("Distance to wall from 0.0 0.0 in direction 1.0 0.0: %.3lf",
           _wall_map->DistanceToWall(K::Ray_2(Point_2(0.0,0.0), K::Direction_2(1.0,1.00000))));
  // Initialize distribution:
  ros::Duration(5.0).sleep(); // Sleep for 5 sec to allow gui node to initialize.
  for (const K::Triangle_2 &tri : _wall_map->_triangles) {
    GUIPolyMsg new_poly;
    new_poly.numVertices = 3;
    for (int i = 0; i < 3; ++i) {
      new_poly.x.push_back(CGAL::to_double(tri[i].x()));
      new_poly.y.push_back(CGAL::to_double(tri[i].y()));
    }
    new_poly.c.r = 255;
    new_poly.c.g = 0;
    new_poly.c.b = 0;
    new_poly.closed = 1;
    _guipoly_pub.publish(new_poly);
  }
}

double logGaussian(double x, double mu, double var) {
  return -0.5*log(2.0*M_PI*var) - (x-mu)*(x-mu)/var/2.0;
}
double logGaussian(double x, double y, double mux, double muy, double var) {
  return -0.5*log(2.0*M_PI*var) - ((x-mux)*(x-mux)+(y-muy)*(y-muy))/var/2.0;
}

void Localization::NormalizeBeliefs() {
  // Normalize the result
  double normalizer = 0.0;
  for (const Particle &par : _particles) {
    normalizer += exp(par.belief);
  }
  for (Particle &par : _particles) {
    par.belief -= log(normalizer);
  }
}

void Localization::onOdometryUpdate(const OdometryMsg::ConstPtr &odo) {
  double dx = odo->x-_prev_odo_x;
  double dy = odo->y-_prev_odo_y;
  double dt = odo->theta-_prev_odo_t;
  _prev_odo_x = odo->x;
  _prev_odo_y = odo->y;
  _prev_odo_t = odo->theta;
  _prev_odo_time = curTime();

  double start_time = curTime();
  ROS_DEBUG_STREAM("onOdometryUpdate: " << dx << " " << dy << " dt: " << dt << " at time: " << curTime()-_time);

  double varD = pow(min(0.03, 0.01 + sqrt(dx*dx+dy*dy)), 2.0);
  double varT = pow(min(0.17453292519, 0.17453292519/20.0+fabs(dt)), 2.0);

  default_random_engine gen;
  normal_distribution<double> xyDist(0.0, varD);
  normal_distribution<double> tDist(0.0, varT);

  vector<Particle> new_particles;
  for (const Particle &par : _particles) {
    new_particles.push_back(Particle(par.x+dx, par.y+dy, par.t+dt,
          par.belief + logGaussian(0.0, 0.0, 0.0, 0.0, varD) + logGaussian(0.0, 0.0, varT)));
    // Generate 10 random points to account for noise.
    for (int i = 0; i < 10; ++i) {
      double x = xyDist(gen) + par.x+dx;
      double y = xyDist(gen) + par.y+dy;
      double t = tDist(gen) + par.t+dt;
      new_particles.push_back(Particle(x, y, t,
            par.belief + logGaussian(x-par.x-dx, 0.0, y-par.y-dy, 0.0, varD) + logGaussian(t-par.t-dt, 0.0, varT)));
    }
  }

  ROS_DEBUG("onOdometryUpdate: time up to generation %.3lf sec.", curTime()-start_time);
  // Substitute old particles with top #N new ones.
  sort(new_particles.begin(), new_particles.end());
  reverse(new_particles.begin(), new_particles.end());
  new_particles.resize(N);
  _particles = new_particles;
  ROS_DEBUG("onOdometryUpdate: time up to sorting %.3lf sec.", curTime()-start_time);

  NormalizeBeliefs();
  PublishLocation();
  ROS_DEBUG("onOdometryUpdate: time passed %.3lf sec.", curTime()-start_time);
}

Vector_2 Rotate(Vector_2 vec, double alfa) {
  return Vector_2(vec.x()*cos(alfa) - vec.y()*sin(alfa),
                  vec.x()*sin(alfa) + vec.y()*cos(alfa));
}

void Localization::onSonarUpdate(const SonarMsg::ConstPtr &son) {
  ROS_DEBUG_STREAM("Got sonar update: " << son->sonarId << " range: " << son->range);
  return;
  double start_time = curTime();
  double varD = 0.01;
  for (Particle &par : _particles) {
    Vector_2 dir(Rotate(SONAR_DIR[son->sonarId], par.t));
    Point_2 sonarLoc(Point_2(par.x, par.y) + Rotate(SONAR_POS[son->sonarId], par.t));
    double mu = _wall_map->DistanceToWall(Ray_2(sonarLoc, dir));
    par.belief += logGaussian(son->range, mu, varD);
  }
  NormalizeBeliefs();
  PublishLocation();
  ROS_DEBUG("onSonarUpdate: time passed %.3lf sec.", curTime()-start_time);
}

double NormalizeRad(double rad) {
  rad = fmod(rad, 2*M_PI);
  if (rad < 0) {
    rad += 2*M_PI;
  }
  return rad;
}

} // namespace navigation
<|endoftext|>


<commit_before>/*
 * (C) Copyright 2013 Kurento (http://kurento.org/)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include "classifier.h"

#define FACE_CASCADE "/usr/share/opencv/lbpcascades/lbpcascade_frontalface.xml"

#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace cv;
class Classifier
{
public:
  Classifier ();
  ~Classifier() {};

  CascadeClassifier face_cascade;
};

Classifier::Classifier()
{
  face_cascade.load ( FACE_CASCADE );
}

static Classifier lbpClassifier = Classifier ();

void classify_image (IplImage *img, CvSeq *facesList)
{
  std::vector<Rect> faces;
  Mat frame (img);
  Mat frame_gray;

  cvtColor ( frame, frame_gray, COLOR_BGR2GRAY );
  equalizeHist ( frame_gray, frame_gray );

  lbpClassifier.face_cascade.detectMultiScale ( frame_gray, faces, 1.2, 3, 0,
      Size (frame.cols / 20, frame.rows / 20),
      Size (frame.cols / 2, frame.rows / 2) );

  for ( size_t i = 0; i < faces.size(); i++ ) {
    CvRect aux = cvRect (faces[i].x, faces[i].y, faces[i].width, faces[i].height);
    cvSeqPush (facesList, &aux);
  }

  faces.clear();
}
<commit_msg>clang-tidy fix: [modernize-loop-convert]<commit_after>/*
 * (C) Copyright 2013 Kurento (http://kurento.org/)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include "classifier.h"

#define FACE_CASCADE "/usr/share/opencv/lbpcascades/lbpcascade_frontalface.xml"

#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace cv;
class Classifier
{
public:
  Classifier ();
  ~Classifier() {};

  CascadeClassifier face_cascade;
};

Classifier::Classifier()
{
  face_cascade.load ( FACE_CASCADE );
}

static Classifier lbpClassifier = Classifier ();

void classify_image (IplImage *img, CvSeq *facesList)
{
  std::vector<Rect> faces;
  Mat frame (img);
  Mat frame_gray;

  cvtColor ( frame, frame_gray, COLOR_BGR2GRAY );
  equalizeHist ( frame_gray, frame_gray );

  lbpClassifier.face_cascade.detectMultiScale ( frame_gray, faces, 1.2, 3, 0,
      Size (frame.cols / 20, frame.rows / 20),
      Size (frame.cols / 2, frame.rows / 2) );

  for (auto &face : faces) {
    CvRect aux = cvRect(face.x, face.y, face.width, face.height);
    cvSeqPush (facesList, &aux);
  }

  faces.clear();
}
<|endoftext|>


<commit_before>/**
 *  Copyright (c) 2014-present, Facebook, Inc.
 *  All rights reserved.
 *
 *  This source code is licensed under both the Apache 2.0 license (found in the
 *  LICENSE file in the root directory of this source tree) and the GPLv2 (found
 *  in the COPYING file in the root directory of this source tree).
 *  You may select, at your option, one of the above-listed licenses.
 */

#include <functional>
#include <string>

#include "osquery/core/conversions.h"
#include "osquery/core/hashing.h"

#include <sqlite3.h>

namespace osquery {

static void hashSqliteValue(sqlite3_context* ctx,
                            int argc,
                            sqlite3_value** argv,
                            HashType ht) {
  if (argc == 0) {
    return;
  }

  if (SQLITE_NULL == sqlite3_value_type(argv[0])) {
    sqlite3_result_null(ctx);
    return;
  }

  // Parse and verify the split input parameters.
  std::string input((char*)sqlite3_value_text(argv[0]));

  auto result = hashFromBuffer(ht, input.data(), input.size());
  sqlite3_result_text(
      ctx, result.c_str(), static_cast<int>(result.size()), SQLITE_TRANSIENT);
}

static void sqliteMD5Func(sqlite3_context* context,
                          int argc,
                          sqlite3_value** argv) {
  hashSqliteValue(context, argc, argv, HASH_TYPE_MD5);
}

static void sqliteSHA1Func(sqlite3_context* context,
                           int argc,
                           sqlite3_value** argv) {
  hashSqliteValue(context, argc, argv, HASH_TYPE_SHA1);
}

static void sqliteSHA256Func(sqlite3_context* context,
                             int argc,
                             sqlite3_value** argv) {
  hashSqliteValue(context, argc, argv, HASH_TYPE_SHA256);
}

void registerHashingExtensions(sqlite3* db) {
  sqlite3_create_function(db,
                          "md5",
                          1,
                          SQLITE_UTF8 | SQLITE_DETERMINISTIC,
                          nullptr,
                          sqliteMD5Func,
                          nullptr,
                          nullptr);
  sqlite3_create_function(db,
                          "sha1",
                          1,
                          SQLITE_UTF8 | SQLITE_DETERMINISTIC,
                          nullptr,
                          sqliteSHA1Func,
                          nullptr,
                          nullptr);
  sqlite3_create_function(db,
                          "sha256",
                          1,
                          SQLITE_UTF8 | SQLITE_DETERMINISTIC,
                          nullptr,
                          sqliteSHA256Func,
                          nullptr,
                          nullptr);
}
} // namespace osquery
<commit_msg>get rid of unnecessary string conversion (#4506)<commit_after>/**
 *  Copyright (c) 2014-present, Facebook, Inc.
 *  All rights reserved.
 *
 *  This source code is licensed under both the Apache 2.0 license (found in the
 *  LICENSE file in the root directory of this source tree) and the GPLv2 (found
 *  in the COPYING file in the root directory of this source tree).
 *  You may select, at your option, one of the above-listed licenses.
 */

#include <functional>
#include <string>

#include "osquery/core/conversions.h"
#include "osquery/core/hashing.h"

#include <sqlite3.h>

namespace osquery {

static void hashSqliteValue(sqlite3_context* ctx,
                            int argc,
                            sqlite3_value** argv,
                            HashType ht) {
  if (argc == 0) {
    return;
  }

  if (SQLITE_NULL == sqlite3_value_type(argv[0])) {
    sqlite3_result_null(ctx);
    return;
  }

  // Parse and verify the split input parameters.
  const char* input =
      reinterpret_cast<const char*>(sqlite3_value_text(argv[0]));

  auto result = hashFromBuffer(ht, input, strlen(input));
  sqlite3_result_text(
      ctx, result.c_str(), static_cast<int>(result.size()), SQLITE_TRANSIENT);
}

static void sqliteMD5Func(sqlite3_context* context,
                          int argc,
                          sqlite3_value** argv) {
  hashSqliteValue(context, argc, argv, HASH_TYPE_MD5);
}

static void sqliteSHA1Func(sqlite3_context* context,
                           int argc,
                           sqlite3_value** argv) {
  hashSqliteValue(context, argc, argv, HASH_TYPE_SHA1);
}

static void sqliteSHA256Func(sqlite3_context* context,
                             int argc,
                             sqlite3_value** argv) {
  hashSqliteValue(context, argc, argv, HASH_TYPE_SHA256);
}

void registerHashingExtensions(sqlite3* db) {
  sqlite3_create_function(db,
                          "md5",
                          1,
                          SQLITE_UTF8 | SQLITE_DETERMINISTIC,
                          nullptr,
                          sqliteMD5Func,
                          nullptr,
                          nullptr);
  sqlite3_create_function(db,
                          "sha1",
                          1,
                          SQLITE_UTF8 | SQLITE_DETERMINISTIC,
                          nullptr,
                          sqliteSHA1Func,
                          nullptr,
                          nullptr);
  sqlite3_create_function(db,
                          "sha256",
                          1,
                          SQLITE_UTF8 | SQLITE_DETERMINISTIC,
                          nullptr,
                          sqliteSHA256Func,
                          nullptr,
                          nullptr);
}
} // namespace osquery
<|endoftext|>


<commit_before>#include "rosplan_planning_system/ProblemGeneration/ProblemInterface.h"
#include <fstream>
#include <sstream>
#include <string>
#include <ctime>
#include <string>
#include <streambuf>

namespace KCL_rosplan {

	/*-------------*/
	/* constructor */
	/*-------------*/

	ProblemInterface::ProblemInterface(ros::NodeHandle& nh)
	{
		node_handle = &nh;

		// connecting to KB
		std::string kb = "knowledge_base";
		node_handle->getParam("knowledge_base", kb);

		// Check problem fle extension to select a problem generator
		std::string planning_lang;
		node_handle->param<std::string>("planning_language", planning_lang, "");
		std::transform(planning_lang.begin(), planning_lang.end(), planning_lang.begin(), ::tolower); // Convert to lowercase
		node_handle->param<std::string>("problem_path", problem_path, "");

		if (problem_path.empty() and planning_lang.empty()) {
			ROS_ERROR("KCL: (%s) Parameters problem_path and planning_language are both undefined. At least one of them must be defined.",
					  ros::this_node::getName().c_str());
			ros::shutdown();
		}

		KCL_rosplan::ProblemGeneratorFactory::ProbGen pg_type;

        if (not problem_path.empty()) {
            std::string extension = (problem_path.size() > 5) ? problem_path.substr(problem_path.find_last_of('.')) : "";
            if (extension == ".pddl" or extension == ".ppddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::PDDL;
            else if (extension == ".rddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::RDDL;
            else {
                ROS_ERROR("KCL: (%s) Unexpected problem file extension %s. Please provide a problem file written in PDDL (.pddl extension) or RDDL (.rddl extension).",
                          ros::this_node::getName().c_str(), extension.c_str());
                ros::shutdown();
            }
        }
        else if (not planning_lang.empty()) {
			if (planning_lang == "pddl" or planning_lang == "ppddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::PDDL;
			else if (planning_lang == "rddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::RDDL;
			else {
				ROS_ERROR("KCL: (%s) Unexpected planning language %s. Please specify the planning language as either \"PDDL\" or \"RDDL\".",
						  ros::this_node::getName().c_str(), planning_lang.c_str());
				ros::shutdown();
			}
		}
        else {
            ROS_ERROR("KCL: (%s) Neither problem file nor planning language was specified! Please specify the parameter to know the planning language.",
                      ros::this_node::getName().c_str());
            ros::shutdown();
		}
		problem_generator = KCL_rosplan::ProblemGeneratorFactory::createProblemGenerator(pg_type, kb);

		// publishing "problem"
		std::string problem_instance = "problem_instance";
		node_handle->getParam("problem_topic", problem_instance);
		problem_publisher = node_handle->advertise<std_msgs::String>(problem_instance, 1, true);
	}

	ProblemInterface::~ProblemInterface()
	{

	}

	/*--------------------*/
	/* Problem interface */
	/*--------------------*/

	/**
	 * problem generation service method (1)
	 * loads parameters from param server
	 */
	bool ProblemInterface::runProblemServerDefault(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {

		// defaults
		problem_path = "common/problem.pddl";

		// load params
		node_handle->getParam("problem_path", problem_path);

		// call problem server
		return runProblemServer(problem_path);
	}

	/**
	 * problem generation service method (2)
	 * loads parameters from service request
	 */
	bool ProblemInterface::runProblemServerParams(rosplan_dispatch_msgs::ProblemService::Request &req, rosplan_dispatch_msgs::ProblemService::Response &res) {
		// call problem server
		bool success = runProblemServer(req.problem_path);
		if(req.problem_string_response) {
			std::ifstream problemIn(req.problem_path.c_str());
			if(problemIn) res.problem_string = std::string(std::istreambuf_iterator<char>(problemIn), std::istreambuf_iterator<char>());
		}
		return success;
	}

	/**
	 * planning system; prepares planning; calls planner; parses plan.
	 */
	bool ProblemInterface::runProblemServer(std::string problemPath) {

		ros::NodeHandle nh("~");

		// save parameter
		problem_path = problemPath;

		// set problem name for ROS_INFO
		std::size_t lastDivide = problem_path.find_last_of("/\\");
		if(lastDivide != std::string::npos) {
			problem_name = problem_path.substr(lastDivide+1);
		} else {
			problem_name = problem_path;
		}

		ROS_INFO("KCL: (%s) (%s) Generating problem file.", ros::this_node::getName().c_str(), problem_name.c_str());
		problem_generator->generateProblemFile(problem_path);
		ROS_INFO("KCL: (%s) (%s) The problem was generated.", ros::this_node::getName().c_str(), problem_name.c_str());

		// publish problem
		std::ifstream problemIn(problem_path.c_str());
		if(problemIn) {
			std_msgs::String problemMsg;
			problemMsg.data = std::string(std::istreambuf_iterator<char>(problemIn), std::istreambuf_iterator<char>());
			problem_publisher.publish(problemMsg);
		}

		return true;
	}
} // close namespace

	/*-------------*/
	/* Main method */
	/*-------------*/

	int main(int argc, char **argv) {

		srand (static_cast <unsigned> (time(0)));

		ros::init(argc,argv,"rosplan_problem_interface");
		ros::NodeHandle nh("~");

		KCL_rosplan::ProblemInterface ProblemInterface(nh);

		// start the planning services
		ros::ServiceServer service1 = nh.advertiseService("problem_generation_server", &KCL_rosplan::ProblemInterface::runProblemServerDefault, &ProblemInterface);
		ros::ServiceServer service2 = nh.advertiseService("problem_generation_server_params", &KCL_rosplan::ProblemInterface::runProblemServerParams, &ProblemInterface);

		ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str());
		ros::spin();

		return 0;
	}
<commit_msg>Changed precedence of problem generator parameters<commit_after>#include "rosplan_planning_system/ProblemGeneration/ProblemInterface.h"
#include <fstream>
#include <sstream>
#include <string>
#include <ctime>
#include <string>
#include <streambuf>

namespace KCL_rosplan {

	/*-------------*/
	/* constructor */
	/*-------------*/

	ProblemInterface::ProblemInterface(ros::NodeHandle& nh)
	{
		node_handle = &nh;

		// connecting to KB
		std::string kb = "knowledge_base";
		node_handle->getParam("knowledge_base", kb);

		// Check problem fle extension to select a problem generator
		std::string planning_lang;
		node_handle->param<std::string>("planning_language", planning_lang, "");
		std::transform(planning_lang.begin(), planning_lang.end(), planning_lang.begin(), ::tolower); // Convert to lowercase
		node_handle->param<std::string>("problem_path", problem_path, "");

		if (problem_path.empty() and planning_lang.empty()) {
			ROS_ERROR("KCL: (%s) Parameters problem_path and planning_language are both undefined. At least one of them must be defined.",
					  ros::this_node::getName().c_str());
			ros::shutdown();
		}

		KCL_rosplan::ProblemGeneratorFactory::ProbGen pg_type;

		if (not planning_lang.empty()) {
			if (planning_lang == "pddl" or planning_lang == "ppddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::PDDL;
			else if (planning_lang == "rddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::RDDL;
			else {
				ROS_WARN("KCL: (%s) Unexpected planning language %s. Please specify the planning language as either \"PDDL\" or \"RDDL\".",
						  ros::this_node::getName().c_str(), planning_lang.c_str());
			}
		} // If roblem path is specified, it'll overwrite the setting from the planning_lang parameter.
        if (not problem_path.empty()) {
            std::string extension = (problem_path.size() > 5) ? problem_path.substr(problem_path.find_last_of('.')) : "";
            if (extension == ".pddl" or extension == ".ppddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::PDDL;
            else if (extension == ".rddl") pg_type = KCL_rosplan::ProblemGeneratorFactory::RDDL;
            else {
                ROS_ERROR("KCL: (%s) Unexpected problem file extension %s. Please provide a problem file written in PDDL (.pddl extension) or RDDL (.rddl extension).",
                          ros::this_node::getName().c_str(), extension.c_str());
                ros::shutdown();
            }
        }
        else {
            ROS_ERROR("KCL: (%s) Neither problem file nor planning language was specified! Please specify the parameter to know the planning language.",
                      ros::this_node::getName().c_str());
            ros::shutdown();
		}
		problem_generator = KCL_rosplan::ProblemGeneratorFactory::createProblemGenerator(pg_type, kb);

		// publishing "problem"
		std::string problem_instance = "problem_instance";
		node_handle->getParam("problem_topic", problem_instance);
		problem_publisher = node_handle->advertise<std_msgs::String>(problem_instance, 1, true);
	}

	ProblemInterface::~ProblemInterface()
	{

	}

	/*--------------------*/
	/* Problem interface */
	/*--------------------*/

	/**
	 * problem generation service method (1)
	 * loads parameters from param server
	 */
	bool ProblemInterface::runProblemServerDefault(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {

		// defaults
		problem_path = "common/problem.pddl";

		// load params
		node_handle->getParam("problem_path", problem_path);

		// call problem server
		return runProblemServer(problem_path);
	}

	/**
	 * problem generation service method (2)
	 * loads parameters from service request
	 */
	bool ProblemInterface::runProblemServerParams(rosplan_dispatch_msgs::ProblemService::Request &req, rosplan_dispatch_msgs::ProblemService::Response &res) {
		// call problem server
		bool success = runProblemServer(req.problem_path);
		if(req.problem_string_response) {
			std::ifstream problemIn(req.problem_path.c_str());
			if(problemIn) res.problem_string = std::string(std::istreambuf_iterator<char>(problemIn), std::istreambuf_iterator<char>());
		}
		return success;
	}

	/**
	 * planning system; prepares planning; calls planner; parses plan.
	 */
	bool ProblemInterface::runProblemServer(std::string problemPath) {

		ros::NodeHandle nh("~");

		// save parameter
		problem_path = problemPath;

		// set problem name for ROS_INFO
		std::size_t lastDivide = problem_path.find_last_of("/\\");
		if(lastDivide != std::string::npos) {
			problem_name = problem_path.substr(lastDivide+1);
		} else {
			problem_name = problem_path;
		}

		ROS_INFO("KCL: (%s) (%s) Generating problem file.", ros::this_node::getName().c_str(), problem_name.c_str());
		problem_generator->generateProblemFile(problem_path);
		ROS_INFO("KCL: (%s) (%s) The problem was generated.", ros::this_node::getName().c_str(), problem_name.c_str());

		// publish problem
		std::ifstream problemIn(problem_path.c_str());
		if(problemIn) {
			std_msgs::String problemMsg;
			problemMsg.data = std::string(std::istreambuf_iterator<char>(problemIn), std::istreambuf_iterator<char>());
			problem_publisher.publish(problemMsg);
		}

		return true;
	}
} // close namespace

	/*-------------*/
	/* Main method */
	/*-------------*/

	int main(int argc, char **argv) {

		srand (static_cast <unsigned> (time(0)));

		ros::init(argc,argv,"rosplan_problem_interface");
		ros::NodeHandle nh("~");

		KCL_rosplan::ProblemInterface ProblemInterface(nh);

		// start the planning services
		ros::ServiceServer service1 = nh.advertiseService("problem_generation_server", &KCL_rosplan::ProblemInterface::runProblemServerDefault, &ProblemInterface);
		ros::ServiceServer service2 = nh.advertiseService("problem_generation_server_params", &KCL_rosplan::ProblemInterface::runProblemServerParams, &ProblemInterface);

		ROS_INFO("KCL: (%s) Ready to receive", ros::this_node::getName().c_str());
		ros::spin();

		return 0;
	}
<|endoftext|>