/*
* Create configuration structures from configuration file
*/
tls_domains_cfg_t* tls_load_config(str* filename)
{
cfg_parser_t* parser;
str empty;
struct stat file_status;
char tmp_name[13] = "configXXXXXX";
str filename_str;
DIR *dir;
struct dirent *ent;
int out_fd, in_fd, filename_is_directory;
char *file_path, ch;
parser = NULL;
memset(&file_status, 0, sizeof(struct stat));
dir = (DIR *)NULL;
in_fd = out_fd = filename_is_directory = 0;
file_path = (char *)0;
if ((cfg = tls_new_cfg()) == NULL) goto error;
if (stat(filename->s, &file_status) != 0) {
LOG(L_ERR, "cannot stat config file %s\n", filename->s);
goto error;
}
if (S_ISDIR(file_status.st_mode)) {
filename_is_directory = 1;
dir = opendir(filename->s);
if (dir == NULL) {
LOG(L_ERR, "cannot open directory file %s\n", filename->s);
goto error;
}
out_fd = mkstemp(&(tmp_name[0]));
if (out_fd == -1) {
LOG(L_ERR, "cannot make tmp file %s\n", &(tmp_name[0]));
goto error;
}
while ((ent = readdir(dir)) != NULL) {
file_path = pkg_malloc(filename->len + 1 + 256);
memcpy(file_path, filename->s, filename->len);
file_path[filename->len] = '/';
strcpy(file_path + filename->len + 1, ent->d_name);
if (stat(file_path, &file_status) != 0) {
LOG(L_ERR, "cannot get status of config file %s\n",
file_path);
goto error;
}
if (S_ISREG(file_status.st_mode)) {
in_fd = open(file_path, O_RDONLY);
if (in_fd == -1) {
LOG(L_ERR, "cannot open config file %s\n",
file_path);
goto error;
}
pkg_free(file_path);
while (read(in_fd, &ch, 1)) {
write(out_fd, &ch, 1);
}
close(in_fd);
in_fd = 0;
ch = '\n';
write(out_fd, &ch, 1);
}
}
closedir(dir);
close(out_fd);
dir = (DIR *)NULL;
out_fd = 0;
}
empty.s = 0;
empty.len = 0;
if (filename_is_directory) {
filename_str.s = &(tmp_name[0]);
filename_str.len = strlen(&(tmp_name[0]));
if ((parser = cfg_parser_init(&empty, &filename_str)) == NULL) {
ERR("tls: Error while initializing configuration file parser.\n");
unlink(&(tmp_name[0]));
goto error;
}
unlink(&(tmp_name[0]));
} else {
if ((parser = cfg_parser_init(&empty, filename)) == NULL) {
ERR("tls: Error while initializing configuration file parser.\n");
goto error;
}
}
cfg_section_parser(parser, parse_domain, NULL);
if (sr_cfg_parse(parser)) goto error;
cfg_parser_close(parser);
return cfg;
error:
if (dir) closedir(dir);
if (out_fd > 0) {
close(out_fd);
unlink(&(tmp_name[0]));
}
if (file_path) pkg_free(file_path);
if (parser) cfg_parser_close(parser);
if (cfg) tls_free_cfg(cfg);
return 0;
}
int main(int argc, char *argv[])
{
char cfg[MAX_FILE_NAME_SIZE]="config.json";
float lPow,rPow;
int state=STOP, stoppedState=RUN;
int beaconToFollow=0;
int ret = 0;
rob_cfg_t rob_cfg;
rob_state_t rob_state;
struct beaconMeasure beacon;
int totalBeacons = 0,curGroundSensor = -1;
double elapsed1 = 0.0, elapsed2 = 0.0, realTotal = 0.0;
struct timeval t1, t2, t3;
bool firstTimeStart = 1;
memset(&rob_state, 0, sizeof(rob_state_t));
/* processing arguments */
while (argc > 2)
{
if (strcmp(argv[1], "-cfg") == 0)
{
strncpy(cfg, argv[2], 99);
cfg[MAX_FILE_NAME_SIZE-1]='\0';
}
else
{
break; /* the while */
}
argc -= 2;
argv += 2;
}
cfg_parser_parse(cfg, &rob_cfg);
// int i;
// for(i = 0; i < rob_cfg.rob_viewer_size; i++)
// printf("Viewer: %s:%d\n", rob_cfg.rob_viewers[i].hostname, rob_cfg.rob_viewers[i].port);
InitJoystick(rob_cfg.joys_dev);
cfg_parser_connect_viewers(&rob_cfg);
/* Connect Robot to simulator */
if( InitRobot(rob_cfg.robo_name, rob_cfg.robo_pos, rob_cfg.hostname) == -1)
{
ret = 1;
printf( "%s Failed to connect\n", rob_cfg.robo_name);
}
else
{
totalBeacons = GetNumberOfBeacons();
printf( "Connected: %s, Total beacons: %d\n", rob_cfg.robo_name, totalBeacons);
state=STOP;
while(1)
{
/* Reading next values from Sensors */
ReadSensors();
if(GetFinished()) /* Simulator has received Finish() or Robot Removed */
{
printf( "Exiting: %s\n", rob_cfg.robo_name );
state = FINISHED;
gettimeofday(&t3, NULL);
elapsed2 = _get_elapsed_secs(&t2, &t3);
realTotal = _get_elapsed_secs(&t1, &t3);
printf("to beacon | to start | total | real total\n");
printf("& %.2f & %.2f & %.2f & %.2f \n", elapsed1, elapsed2, elapsed1 + elapsed2, realTotal);
break;
}
if(state==STOP && GetStartButton())
{
state=stoppedState; /* Restart */
if( firstTimeStart )
{
firstTimeStart = 0;
printf("Started counting elapsed time\n");
gettimeofday(&t1, NULL);
}
}
if(state!=STOP && GetStopButton()) {
stoppedState=state;
state=STOP; /* Interrupt */
}
curGroundSensor = GetGroundSensor();
switch (state)
{
case RUN: /* Go */
if( GetVisitingLed() )
{
gettimeofday(&t2, NULL);
elapsed1 = _get_elapsed_secs(&t1, &t2);
printf("Elapsed from origin to beacon: %f\n", elapsed1);
state = WAIT;
DriveMotors(0.0,0.0);
}
else
{
if( curGroundSensor == beaconToFollow )
{
beaconToFollow++;
SetVisitingLed(1);
printf("%s visited target at %d\n", rob_cfg.robo_name, GetTime());
}
else {
DetermineAction(beaconToFollow, &lPow, &rPow);
DriveMotors(lPow, rPow);
}
}
break;
case RETURN: /* Go */
if( curGroundSensor == totalBeacons )
{
printf("%s found home at %d\n", rob_cfg.robo_name, GetTime());
Finish();
}
else {
DetermineAction(beaconToFollow, &lPow, &rPow);
DriveMotors(lPow, rPow);
}
break;
case WAIT: /* Wait for others to visit target */
if(GetReturningLed())
{
SetVisitingLed(0);
state = RETURN;
gettimeofday(&t2, NULL);
}
DriveMotors(0.0,0.0);
break;
}
//Say(rob_cfg.robo_name);
rob_state.state = state;
if( (rob_state.leftAvail = IsObstacleReady(LEFT)) )
rob_state.left = GetObstacleSensor(LEFT);
if( (rob_state.rightAvail = IsObstacleReady(RIGHT)) )
rob_state.right = GetObstacleSensor(RIGHT);
if( (rob_state.centerAvail = IsObstacleReady(CENTER)) )
rob_state.center = GetObstacleSensor(CENTER);
if(IsGPSReady())
{
rob_state.x = GetX();
rob_state.y = GetY();
}
// if( IsGPSDirReady() )
// rob_state.dir = GetDir();
if( IsCompassReady() )
rob_state.dir = GetCompassSensor();
if( ( rob_state.beaconVis = IsBeaconReady(beaconToFollow) ) )
{
beacon = GetBeaconSensor(beaconToFollow);
if( ( rob_state.beaconVis = beacon.beaconVisible ) )
rob_state.beaconDir = beacon.beaconDir;
}
send_all_viewer_state_message(&rob_cfg, &rob_state);
RequestCompassSensor();
//Request Sensors for next cycle
if(GetTime() % 2 == 0) {
RequestObstacleSensor(CENTER);
if( (GetTime() % 8) == 0
|| beaconToFollow == totalBeacons )
RequestGroundSensor();
else
RequestBeaconSensor(beaconToFollow);
}
else {
RequestSensors(2, "IRSensor1", "IRSensor2");
}
}
send_all_viewer_state_message(&rob_cfg, &rob_state);
}
printf("Doing cleanup: %s\n", rob_cfg.robo_name);
CloseAndFreeJoystick();
cfg_parser_close(&rob_cfg);
return ret;
}
