// ------------------------------------------------------------------------------
// 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 perMain()
{
#if defined(PCBSKY9X) && !defined(REVA)
calcConsumption();
#endif
checkSpeakerVolume();
checkEeprom();
sdMountPoll();
writeLogs();
handleUsbConnection();
checkTrainerSettings();
checkBattery();
uint8_t evt = getEvent(false);
if (evt && (g_eeGeneral.backlightMode & e_backlight_mode_keys)) backlightOn(); // on keypress turn the light on
checkBacklight();
#if defined(NAVIGATION_STICKS)
uint8_t sticks_evt = getSticksNavigationEvent();
if (sticks_evt) evt = sticks_evt;
#endif
#if defined(USB_MASS_STORAGE)
if (usbPlugged()) {
// disable access to menus
lcd_clear();
menuMainView(0);
lcdRefresh();
return;
}
#endif
#if defined(LUA)
uint32_t t0 = get_tmr10ms();
static uint32_t lastLuaTime = 0;
uint16_t interval = (lastLuaTime == 0 ? 0 : (t0 - lastLuaTime));
lastLuaTime = t0;
if (interval > maxLuaInterval) {
maxLuaInterval = interval;
}
// run Lua scripts that don't use LCD (to use CPU time while LCD DMA is running)
luaTask(0, RUN_MIX_SCRIPT | RUN_FUNC_SCRIPT | RUN_TELEM_BG_SCRIPT, false);
// wait for LCD DMA to finish before continuing, because code from this point
// is allowed to change the contents of LCD buffer
//
// WARNING: make sure no code above this line does any change to the LCD display buffer!
//
lcdRefreshWait();
// draw LCD from menus or from Lua script
// run Lua scripts that use LCD
bool standaloneScriptWasRun = luaTask(evt, RUN_STNDAL_SCRIPT, true);
bool refreshScreen = true;
if (!standaloneScriptWasRun) {
refreshScreen = !luaTask(evt, RUN_TELEM_FG_SCRIPT, true);
}
t0 = get_tmr10ms() - t0;
if (t0 > maxLuaDuration) {
maxLuaDuration = t0;
}
if (!standaloneScriptWasRun)
#else
lcdRefreshWait(); // WARNING: make sure no code above this line does any change to the LCD display buffer!
const bool refreshScreen = true;
#endif
{
// normal GUI from menus
const char *warn = s_warning;
uint8_t menu = s_menu_count;
if (refreshScreen) {
lcd_clear();
}
if (menuEvent) {
m_posVert = menuEvent == EVT_ENTRY_UP ? g_menuPos[g_menuStackPtr] : 0;
m_posHorz = 0;
evt = menuEvent;
menuEvent = 0;
AUDIO_MENUS();
}
g_menuStack[g_menuStackPtr]((warn || menu) ? 0 : evt);
if (warn) DISPLAY_WARNING(evt);
if (menu) {
const char * result = displayMenu(evt);
if (result) {
menuHandler(result);
putEvent(EVT_MENU_UP);
}
}
drawStatusLine();
}
lcdRefresh();
#if defined(REV9E) && !defined(SIMU)
topLcdRefreshStart();
setTopFirstTimer(getValue(MIXSRC_FIRST_TIMER+g_model.topLcdTimer));
setTopSecondTimer(g_eeGeneral.globalTimer + sessionTimer);
setTopRssi(TELEMETRY_RSSI());
setTopBatteryValue(g_vbat100mV);
setTopBatteryState(GET_TXBATT_BARS(), IS_TXBATT_WARNING());
topLcdRefreshEnd();
#endif
#if defined(REV9E) && !defined(SIMU)
bluetoothWakeup();
#endif
#if defined(PCBTARANIS)
if (requestScreenshot) {
requestScreenshot = false;
writeScreenshot();
}
#endif
}
