States::States() {
// Set my relevant indices and print out a debug message
ispin = thisIndex.x, ikpt = thisIndex.y, istate = thisIndex.z;
//CkPrintf("State chare with spin, kpt, state (%d %d %d) constructed on PE %d.\n", ispin, ikpt, istate, CkMyPe());
// Get access to the global struct of readonlies
GWBSE *gwbse = GWBSE::get();
// set the option for the type of file read
ibinary_opt = gwbse->gwbseopts.ibinary_opt;
// set file name to read my state from
sprintf(fileName, "./STATES_IN/Spin.%d_Kpt.%d_Bead.0_Temper.0/state%d.out", ispin, ikpt, istate+1);
// if doublePack is true -> gamma point only calculation (only 1 k point)
// if doublePack is false -> multiple k points
// double pack is hard coded. FIXME : minjung needs to change this
doublePack = false;
// read states from file
readState(fileName);
// read states from shifted k grid (for q=0 calculation)
// set file name to read my state from (there is additional "0" right after "Kpt." )
// we do this only for the occupied states
nocc = gwbse->gw_epsilon.nocc;
int qindex = Q_IDX;
if( istate < nocc && qindex == 0){
sprintf(fileName,"./STATES_IN/Spin.%d_Kpt.0%d_Bead.0_Temper.0/state%d.out", ispin, ikpt, istate+1);
readStateShifted(fileName);
}
// set the size of fft grid
for(int i=0;i<3;i++){nfft[i] = gwbse->gw_parallel.fft_nelems[i];}
}
static void
loadState (CompDisplay *d,
char *previousId)
{
xmlDocPtr doc;
xmlNodePtr root;
char *filename;
struct passwd *p = getpwuid (geteuid ());
/* setup filename */
filename = malloc (sizeof (char) *
(strlen (p->pw_dir) + strlen (previousId) + 18));
if (!filename)
return;
sprintf (filename, "%s/.compiz/session/%s", p->pw_dir, previousId);
doc = xmlParseFile (filename);
free (filename);
if (!doc)
return;
root = xmlDocGetRootElement (doc);
if (root && xmlStrcmp (root->name, BAD_CAST "compiz_session") == 0)
readState (root);
xmlFreeDoc (doc);
xmlCleanupParser ();
}
void XmlReader::readNote()
{
while(!reader.atEnd())
{
reader.readNext();
if(reader.isStartElement())
{
if("name"==reader.name())
{
readName();
}
else if("passwd" == reader.name())
{
readPasswd();
}
else if("state" == reader.name())
{
readState();
}
else if("remember" == reader.name())
{
readRememberPasswd();
}
else if("auto" == reader.name())
{
readAtuoLogin();
}
}
}
}
/*
*
* name: main
*
* Recieves information from the user of the input and output files, and then
* makes appropriate calls.
*
* @param argc the number of arguments passed (including the program)
* @param argv the argument array of the program call
* @return error code
*/
int main(int argc, char ** argv){
// The user can pass a parameter to the program for the file name.
// If no parameter is given, the program will ask explicitly.
char input[MAX_FILE_LEN];
if(argc == 2){
strcpy(input, argv[1]);
}
else{
printf("\n Name of your input file (%d characters max): ", MAX_FILE_LEN);
scanf("%s", input);
}
struct stateNode init;
readState(&init, input);
struct stateNode goal = {
0, // heuristic
{GOAL_STATE}, // state
0, // pathCost
NULL // predecessor
};
h(&init, &goal);
if(!solvable(&init, &goal)){
printNode(&init);
printf("\nUnsolvable\n\n");
return 0;
}
printNodePath(search(&init, &goal));
return 0;
}
void Player::update(Server& s)
{
while(socket->bytesAvailable()) {
QDataStream is(socket);
if (packetSize<0) {
if (socket->bytesAvailable()<4) break;
is>>packetSize;
}
if (socket->bytesAvailable()<packetSize) break;
packetSize=-1;
quint8 type;
is>>type;
switch(type) {
case MSG_STATE:
readState(is);
break;
case MSG_SHOOT:
readShoot(is, s);
break;
case MSG_INFO:
readInfo(is);
break;
case MSG_CHAT:
readChat(is);
break;
default:
qDebug()<<type;
abort();
}
}
updatePhysics(s);
}
void Connection::update()
{
while(bytesAvailable()) {
QDataStream s(this);
if (packetSize<0) {
if (bytesAvailable()<4) break;
s>>packetSize;
// if (packetSize>1<<20) //qDebug()<<"big packet"<<packetSize;
}
if (bytesAvailable()<packetSize) break;
// //qDebug()<<"packet size"<<packetSize;
packetSize=-1;
quint8 type;
s>>type;
// //qDebug()<<"available: "<<type;
switch(type) {
case MSG_INITIAL:
readInitial(s);
break;
case MSG_STATE:
readState(s);
break;
case MSG_SHOOT:
readShoot(s);
break;
case MSG_HIT:
readHit(s);
break;
case MSG_ENEMY:
readEnemy(s);
break;
case MSG_ITEM:
readItem(s);
break;
case MSG_GET:
readGet(s);
break;
case MSG_DIE:
for(int i=0; i<engine.bulletCounts.size(); ++i) engine.bulletCounts[i]=0;
player->weapon = 1;
break;
case MSG_LIGHTNING:
readLightning(s);
break;
case MSG_STATS:
readStats(s);
break;
case MSG_CHAT:
readChat(s);
break;
default:
//qDebug()<<type;
abort();
}
++packetCount;
}
}
void RotaryEncoder::init()
{
pinMode(pin0, INPUT);
pinMode(pin1, INPUT);
digitalWrite(pin0, HIGH); // enable internal pullup
digitalWrite(pin1, HIGH);
change = 0;
delay(10); // ensure we read the initial state correctly
state = readState();
}
std::vector<MapSelectData> XMLMapListParser::loadXMLFile(const char *xmlFileName)
{
std::vector<MapSelectData> data ;
std::string fullPath = CCFileUtils::sharedFileUtils()->fullPathForFilename(xmlFileName);
unsigned long dataSize = 0;
unsigned char* pFileData = NULL;
pFileData = (unsigned char*) CCFileUtils::sharedFileUtils()->getFileData(fullPath.c_str(), "r", &dataSize);
// CCLOG("Empty file: %s", fullPath.c_str());
if (!pFileData)
{
// CCLOG("Empty file: %s", fullPath.c_str());
return data;
}
std::string fileContent;
fileContent.assign(reinterpret_cast<const char*>( pFileData), dataSize);
XMLDocument document;
if( document.Parse(fileContent.c_str()) != XML_NO_ERROR)
{
CCLOG("Cannot parse file: %s", fullPath.c_str());
return data;
}
//Parse data
XMLElement* docElement = document.FirstChildElement();
if(docElement != NULL)
{
for(XMLElement* child = docElement->FirstChildElement(TAG_MAP) ; child!=NULL ; child = child->NextSiblingElement())
{
MapSelectData mdata;
TextureSelector selector;
selector.activeTexture = selector.deactiveTexture = readNormalIcon(child);
selector.selectedTexture = readSelectedIcon(child);
mdata.setSelector(selector);
mdata.setID(readID(child));
mdata.setstate(readState(child));
mdata.buttonID = XMLHelper::readAttributeString(child, ATTRIBUTE_BUTTON_ID, "s");
data.push_back(mdata);
}
}
delete []pFileData;
return data;
}
//--------------------------------------------------------------------------------------------------------------------------------------
void Encoder::begin()
{
pinMode(pin0, INPUT);
pinMode(pin1, INPUT);
if(pullup)
{
digitalWrite(pin0, HIGH); // включаем подтягивающие резисторы
digitalWrite(pin1, HIGH);
}
change = 0;
delay(10); // читаем состояние после небольшого ожидания
state = readState();
}
void
SlimeConfigResponse::fill()
{
if (_filled) {
LOG(info, "SlimeConfigResponse::fill() called twice, probably a bug");
return;
}
Memory json((*_returnValues)[0]._string._str);
Slime * data = new Slime();
JsonFormat::decode(json, *data);
_data.reset(data);
_key = readKey();
_state = readState();
_value = readConfigValue();
readTrace();
_filled = true;
if (LOG_WOULD_LOG(debug)) {
LOG(debug, "trace at return(%s)", _trace.toString().c_str());
}
}
void RotaryEncoder::poll()
{
// State transition table. Each entry has the following meaning:
// 0 - the encoder hasn't moved
// 1 - the encoder has moved 1 unit clockwise
// -1 = the encoder has moved 1 unit anticlockwise
// 2 = illegal transition, we must have missed a state
static int tbl[16] =
{ 0, +1, -1, 0, // position 3 = 00 to 11, can't really do anythin, so 0
-1, 0, -2, +1, // position 2 = 01 to 10, assume it was a bounce and should be 01 -> 00 -> 10
+1, +2, 0, -1, // position 1 = 10 to 01, assume it was a bounce and should be 10 -> 00 -> 01
0, -1, +1, 0 // position 0 = 11 to 10, can't really do anything
};
unsigned int t = readState();
int movement = tbl[(state << 2) | t];
if (movement != 0)
{
change += movement;
state = t;
}
}
int readCheckpoint(EvaluationState* es)
{
int rc;
FILE* f;
f = mwOpenResolved(CHECKPOINT_FILE, "rb");
if (!f)
{
mwPerror("Opening checkpoint '%s'", CHECKPOINT_FILE);
return 1;
}
rc = readState(f, es);
if (rc)
mw_printf("Failed to read state\n");
fclose(f);
addTmpCheckpointSums(es);
return rc;
}
int readCheckpoint(EvaluationState* es)
{
int rc;
FILE* f;
f = mwOpenResolved(CHECKPOINT_FILE, "rb");
if (!f)
{
perror("Opening checkpoint");
return 1;
}
rc = readState(f, es);
if (rc)
warn("Failed to read state\n");
fclose(f);
/* The GPU checkpoint saved the last checkpoint results in temporaries.
Add to the total from previous episodes. */
addTmpSums(es);
return rc;
}
void FLManagerModules::init() {
#ifndef FL_QUICK_CLIENT
if ( !db_->dbAux() )
return ;
if ( db_->connectionName() != "default" )
return ;
FLTableMetaData *tmpTMD;
tmpTMD = db_->manager()->createSystemTable( "flfiles" );
tmpTMD = db_->manager()->createSystemTable( "flsettings" );
tmpTMD = db_->manager()->createSystemTable( "flserial" );
tmpTMD = db_->manager()->createSystemTable( "flvar" );
FLSqlCursor curSet( "flsettings", true, db_->dbAux() );
curSet.select( "flkey = 'sysmodver'" );
if ( !curSet.first() || curSet.valueBuffer( "valor" ).toString() != QString( VERSION ) ) {
FLDiskCache::clear();
QSqlQuery qry( QString::null, db_->dbAux() );
qry.exec( "DROP TABLE flserial" );
qry.exec( "DROP TABLE flvar" );
if ( curSet.isValid() )
curSet.setModeAccess( FLSqlCursor::EDIT );
else
curSet.setModeAccess( FLSqlCursor::INSERT );
curSet.refreshBuffer();
curSet.setValueBuffer( "flkey", "sysmodver" );
curSet.setValueBuffer( "valor", QString( VERSION ) );
curSet.commitBuffer();
}
tmpTMD = db_->manager()->createSystemTable( "flfiles" );
tmpTMD = db_->manager()->createSystemTable( "flserial" );
tmpTMD = db_->manager()->createSystemTable( "flvar" );
tmpTMD = db_->manager()->createSystemTable( "flareas" );
if ( tmpTMD ) {
FLSqlCursor cursor( "flareas", true, db_->dbAux() );
cursor.setModeAccess( FLSqlCursor::INSERT );
cursor.refreshBuffer();
cursor.setValueBuffer( "idarea", "sys" );
cursor.setValueBuffer( "descripcion", QApplication::tr( "Sistema" ) );
cursor.setValueBuffer( "bloqueo", QVariant( false, 0 ) );
cursor.commitBuffer();
}
tmpTMD = db_->manager()->createSystemTable( "flmodules" );
if ( tmpTMD ) {
FLSqlCursor cursor( "flmodules", true, db_->dbAux() );
cursor.setModeAccess( FLSqlCursor::INSERT );
cursor.refreshBuffer();
cursor.setValueBuffer( "idmodulo", "sys" );
cursor.setValueBuffer( "idarea", "sys" );
cursor.setValueBuffer( "descripcion", QApplication::tr( "Administración" ) );
cursor.setValueBuffer( "icono", contentFS( FLDATA "/sys.xpm" ) );
cursor.setValueBuffer( "bloqueo", QVariant( false, 0 ) );
cursor.commitBuffer();
}
#endif
readState();
}
// ------------------------------------------------------------------------------
// Main
// ------------------------------------------------------------------------------
int main(int argc, char **argv)
{
// This program uses throw, wrap one big try/catch here
ros::init(argc, argv, "ros_ca_flight");
// vxo=0;
// vyo=0;
// vyi=0;
// vxi=0;
// xi=0;
// yi=0;
// yo=0;
// xo=0;
// xt=0;
// yt=0;
dt = 0.1;
// ros::nodeHandler
ros::NodeHandle nh_;
// own_imu_sub = nh_.subscribe("odroid1/mavros/imu/data",1,own_vel_callback);
// own_pose_sub = nh_.subscribe("odometry",1,own_pose_callback);
// own_pose_sub = nh_.subscribe("/demo_odom",1,own_pose_demo_callback);
// own_pose_sub = nh_.subscribe("odroid1/odometry",1,own_pose_callback);
// intruder_imu_sub = nh_.subscribe("odroid2/mavros/imu/data",1,intruder_vel_callback);
// intruder_pose_sub = nh_.subscribe("odroid2/odometry",1,intruder_pose_callback);
// own_pose_sub = nh_.subscribe("mavros/local_position/local",1,gps_own_pose_callback);
own_pose_sub = nh_.subscribe("mavros/position/local",1,gps_own_pose_callback);
own_vel_sub = nh_.subscribe("mavros/global_position/gps_vel",1,gps_own_vel_callback);
own_ptam_sub = nh_.subscribe("vslam/pose",1,ptam_own_pose_callback);
// intruder_pose_sub = nh_.subscribe("odroid1/mavros/local_position/local",1,gps_intruder_pose_callback);
intruder_pose_sub = nh_.subscribe("odroid1/mavros/position/local",1,gps_intruder_pose_callback);
intruder_vel_sub = nh_.subscribe("odroid1/mavros/global_position/gps_vel",1,gps_intruder_vel_callback);
set_pt_vel_pub = nh_.advertise<geometry_msgs::TwistStamped>("/mavros/cmd_vel",1);
set_pt_vel_pub_1 = nh_.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel",1);
set_pt_vel_pub_2 = nh_.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint/cmd_vel",1);
FILE *fpIn, *fpData;
int numVerticies, numElements;
// Needed in decide_Algorithm() but allocated or evaluated
// in start_Algorithm():
int numDims, numActions, *elementsDim;
FILE *fpOut, *fpLog;
double *neighY;
double **discMat, **neighX;
struct cd_grid **gridQsa; // This should hold a vector of grids, one for each action
struct stat st = {0};
struct tm *t;
time_t timeVar;
char str_time[100];
char log_destination[150];
char results_destination[150];
// Variables simply used in the algorithm functions, may be redeclared in each function:
int i, j, k;
int err, keyinput, exitCondition;
if (VERBOSE)
printf("Reading parameter file...\n");
/* Open and check the files */
// fpIn = fopen("CA2_params.prm","r");
// fpData = fopen("CA2_data.dat","r");
fpIn = fopen(argv[1],"r");
fpData = fopen(argv[2],"r");
// fpIn = fopen("paramtemp.txt","r");
// fpData = fopen("datatemp.txt","r");
// fpIn = fopen("test.txt","r");
// fpData = fopen("data.txt","r");
// fpIn = fopen("param8Dim141201.txt","r");
// fpData = fopen("data8Dim141201.txt","r");
if (stat("logs", &st) == -1)
mkdir("logs", 0777);
timeVar = time(NULL);
t = localtime(&timeVar);
strftime(str_time, sizeof(str_time), "%H%M%S",t);
strcpy(results_destination,"./logs/CA2_results_");
strcat(results_destination,str_time);
strcat(results_destination,".out");
fpOut = fopen(results_destination,"w");
strcpy(log_destination,"./logs/CA2_results_");
strcat(log_destination,str_time);
strcat(log_destination,".log");
fpLog = fopen(log_destination,"w");
//printf("Output file name: %s", results_destination);
/* Check for errors opening files */
if (fpIn == NULL)
{ fprintf(stderr, "Can't open input file param\n");
exit(1);
}
if (fpData == NULL)
{ fprintf(stderr, "Can't open data file data\n");
exit(1);
}
if (fpLog == NULL)
{ fprintf(stderr, "Can't open log file: %s\n", log_destination);
exit(1);
}
if (fpOut == NULL)
{ fprintf(stderr, "Can't open output file results out: %s\n", results_destination);
exit(1);
}
/* Read in parameter file */
// Number of dimensions:
fscanf(fpIn, "%d", &numDims);
// Number of grid points (states) in each dimension:
numElements = 1;
elementsDim = (int *)malloc(numDims*sizeof(int));
discMat = (double **)malloc(numDims*sizeof(double));
for (i=0; i<numDims; i++)
{ fscanf(fpIn, "%d", &elementsDim[i]);
//printf("Dimension %d = %d\n", i, elementsDim[i]);
numElements *= elementsDim[i];
discMat[i] = (double *)malloc(elementsDim[i]*sizeof(double));
}
// Values of states at each grid point:
for (i=0; i<numDims; i++)
{ for (j=0; j<elementsDim[i]; j++)
{ fscanf(fpIn, "%lf", &discMat[i][j]);
}
}
// Number of actions:
fscanf(fpIn, "%d", &numActions);
fclose(fpIn);
if (VERBOSE)
printf("Done reading parameter file\n");
/* Finished reading parameter file */
/* Create the grid data structure to hold contents as specified in the parameter file */
//cell_init = (void *)malloc(numElements*sizeof(double));
// gridInst = (struct cd_grid **) malloc(sizeof(struct cd_grid));
double var;
struct cd_grid **gridInst;
int actionInd;
int *subs;
gridInst = (struct cd_grid **) malloc(sizeof(struct cd_grid));
gridQsa = (struct cd_grid **)malloc(numActions*sizeof(struct cd_grid));
subs = (int *)malloc(numDims*sizeof(int));
// To make this a single block of text for every possible value of numDims, need to be able to
// fill the "grid" with elements directly using the index and subscripts. There may already be
// a function in cd_grid to do this...
if (VERBOSE)
printf("Reading data file\n");
if (numDims==3)
{ double Q[elementsDim[0]][elementsDim[1]][elementsDim[2]];
/* Using the contents of the parameter file, read and parse the data file */
for (actionInd=0; actionInd<numActions; actionInd++)
{
// I should probably be able to read in the elements directly to Q if it's allocated into a contiguous
// memory block. Could just use the index value, rather than going from an index to a subscript and
// back to and index? Just need to verify that this works, then make sure my revision matches this version.
for (i=0;i<numElements;i++)
{ fscanf(fpData, "%lf", &var);
err = ind2subs(elementsDim, numDims, i, subs);
Q[subs[0]][subs[1]][subs[2]] = var;
}
// After the number of dimensions, the size of each dimension must be passed in as a series of arguments.
cd_grid_create_fill(gridInst, Q, sizeof(double), numDims, elementsDim[0], elementsDim[1], elementsDim[2]);
gridQsa[actionInd] = *gridInst;
}
}
if (numDims==6)
{ double Q[elementsDim[0]][elementsDim[1]][elementsDim[2]][elementsDim[3]][elementsDim[4]][elementsDim[5]];
for (actionInd=0; actionInd<numActions; actionInd++)
{ for (i=0;i<numElements;i++)
{ fscanf(fpData, "%lf", &var);
err = ind2subs(elementsDim, numDims, i, subs);
Q[subs[0]][subs[1]][subs[2]][subs[3]][subs[4]][subs[5]] = var;
}
cd_grid_create_fill(gridInst, Q, sizeof(double), numDims, elementsDim[0], elementsDim[1], elementsDim[2], elementsDim[3], elementsDim[4], elementsDim[5]);
gridQsa[actionInd] = *gridInst;
}
}
if (numDims==8)
{ double Q[elementsDim[0]][elementsDim[1]][elementsDim[2]][elementsDim[3]][elementsDim[4]][elementsDim[5]][elementsDim[6]][elementsDim[7]];
for (actionInd=0; actionInd<numActions; actionInd++)
{ for (i=0;i<numElements;i++)
{ fscanf(fpData, "%lf", &var);
err = ind2subs(elementsDim, numDims, i, subs);
Q[subs[0]][subs[1]][subs[2]][subs[3]][subs[4]][subs[5]][subs[6]][subs[7]] = var;
}
cd_grid_create_fill(gridInst, Q, sizeof(double), numDims, elementsDim[0], elementsDim[1], elementsDim[2], elementsDim[3], elementsDim[4], elementsDim[5], elementsDim[6], elementsDim[7]);
gridQsa[actionInd] = *gridInst;
}
}
// // Test the read-in data:
// double testArray[numElements];
// for (i=0;i<numElements;i++){
// err = ind2subs(elementsDim, numDims, i, subs);
// testArray[i] = *(double *)cd_grid_get_subs(gridQsa[0], subs);
// printf("%lf\n", testArray[i]);
// }
fclose(fpData);
free(gridInst);
free(subs);
if (VERBOSE)
printf("Done reading data file\n");
// Allocate space for the interpolations needed to decide actions:
numVerticies = pow(2,numDims);
neighX = (double **)malloc(numDims*sizeof(double));
if (neighX)
for (i=0;i<numDims;i++)
neighX[i]=(double *)malloc(numVerticies*sizeof(double));
neighY = (double *)malloc(numVerticies*sizeof(double));
/***********************************************************
*** The following would be in decide_Algorithm() ***
************************************************************/
int bestActionInd;
double maxQsa;
int stepCounter;
double *qVals, *currentState, *indicies;
// These allocations may be done on every call to decide_Algorithm(), they're not large
qVals = (double *)malloc(numActions*sizeof(double));
currentState = (double *)malloc(numDims*sizeof(double));
indicies = (double *)malloc(numDims*sizeof(double));
exitCondition = 0;
stepCounter = 0;
printf("BEGINNING SIMULATION\n");
printf("\n");
// try
// { int result = top(argc,argv);
// return result;
// }
// catch ( int error )
// { fprintf(stderr,"main threw exception %i \n" , error);
// return error;
// }
// if (!(nh_.ok()))
// ros::spin();
// if ((nh_.ok())&&(!exitCondition))
int NOMINAL_MODE_LAST;
NOMINAL_MODE_LAST = 1;
while (!exitCondition)
{
// while ((!exitCondition)&&(nh_.ok()))
// read the current state from MAVLink (or a file, for now)
err = readState(currentState, numDims, (double)stepCounter);
// std::cerr<<"It actually reached here"<<std::endl;
// find indicies into the grid using the current state
err = getIndicies(indicies, currentState, numDims, elementsDim, discMat);
// for each action:
int actionInd;
for (actionInd=0; actionInd<numActions; actionInd++)
{
// read in the neighbor verticies to the data structure and read in the values at the verticies
err = getNeighbors(indicies, &gridQsa[actionInd], neighX, neighY, numDims, discMat);
// interpolate within the data file using the current state (call interpN()) and store
qVals[actionInd] = interpN(numDims, currentState, neighX, neighY, EXTRAPMODE);
if (VERBOSE)
{ printf("Indicies:\n");
printf("[");
for (i=0;i<numDims;i++)
{ printf("%lf, ", indicies[i]);
}
printf("]\n");
printf("Neighbors:\n");
printf("[");
for (i=0;i<numDims;i++)
{ for(j=0;j<numVerticies; j++)
{ printf("%lf, ", neighX[i][j]);
}
printf("\n");
}
printf("Neighbor values:\n");
printf("[");
for (i=0;i<numVerticies;i++)
{ printf("%lf, ", neighY[i]);
}
printf("]\n");
printf("Interpolated Value: %lf\n", qVals[actionInd]);
}
}
// calculate the best action
bestActionInd = 0;
maxQsa = qVals[0];
for (i=1;i<numActions;i++)
{ if (qVals[i] > maxQsa)
{ maxQsa = qVals[i];
bestActionInd = i;
}
}
// write the action to MAVLink (or a file, for now)
if (intruderThreat(currentState))
{
err = writeCAAction(bestActionInd, currentState, numDims, fpOut);
if (NOMINAL_MODE_LAST)
{
// We were just in nominal mode, so record the current position as the start of the desired trajectory
xt = xo;
yt = yo;
printf("Setting nominal trajectory start point: %lf %lf\n\n", xt, yt);
NOMINAL_MODE_LAST = 0;
}
}
else
{
err = writeNominalAction(fpOut);
NOMINAL_MODE_LAST = 1;
}
// log the current state and action to a log file
err = writeLogs(fpLog, stepCounter, currentState, numDims, bestActionInd);
// Increment the counter (proxy for the current time when writing to file)
stepCounter++;
// While debugging, stop after a predetermined number of steps:
// if (stepCounter>=MAXSIMSTEPS)
// { printf("ENDING SIMULATION\n\n");
// exitCondition = 1;
// }
// if (time_to_exit){
// exitCondition = 1;
// }
if (nh_.ok())
ros::spinOnce();
// ros::spin();
else
exitCondition = 1;
usleep(1000000*dt);
}
free(currentState);
free(qVals);
free(indicies);
for (i=0;i<numActions;i++)
{ cd_grid_destroy(gridQsa[i]);
}
free(gridQsa);
for (i=0; i<numDims; i++)
{ free(neighX[i]);
free(discMat[i]);
}
free(discMat);
free(elementsDim);
free(neighX);
free(neighY);
/* close the log and output files */
fclose(fpOut);
fclose(fpLog);
printf("Exiting\n");
return 0;
}
void VacuumEnvironment::updateResults() {
this->performanceMeasure->update(readState());
}
void Unit::load(IO::StreamIn & stream)
{
readState(stream);
controller->load(stream);
}
double Radio::getFrequency() {
readState();
return frequencyToDouble(this->in.parsed.pllH, this->in.parsed.pllL);
}
