double
get(Matrix *matrix, unsigned row, unsigned col)
{
if (!matrix) {
return 0;
}
return matrix->data[midx(matrix, row, col)];
}
void
elementwise(Matrix *A, Matrix *B, char op)
{
if (!A || !B) {
return;
}
unsigned i, j;
if (A->transposed == B->transposed) {
if (A->rows != B->rows || A->cols != B->cols) {
return;
}
} else {
if (A->rows != B->cols || A->cols != B->rows) {
return;
}
}
#pragma omp parallel for
for (j = 0; j < A->cols; ++j) {
for (i = 0; i < A->rows; ++i) {
switch (op) {
case '+':
A->data[midx(A, i, j)] += get(B, i, j);
break;
case '-':
A->data[midx(A, i, j)] -= get(B, i, j);
break;
case '*':
A->data[midx(A, i, j)] *= get(B, i, j);
break;
case '/':
A->data[midx(A, i, j)] /= get(B, i, j) + 1e-6;
break;
default:
break;
}
}
}
}
Matrix *make_matrix_random(int rows, int cols, Random *rnd)
{
if (!rnd) {
return NULL;
}
Matrix *matrix = make_matrix_zero(rows, cols);
if (!matrix) {
return NULL;
}
unsigned i, j;
for (j = 0; j < cols; ++j) {
for (i = 0; i < rows; ++i) {
matrix->data[midx(matrix, i, j)] = rnd->ops->next(rnd);
}
}
return matrix;
}
void
calculate_lcs(it xo, it xlo, it xhi, it ylo, it yhi, members & xs_in_lcs)
{
unsigned const nx = distance(xlo, xhi);
if (nx == 0)
{
// empty range. all done
}
else if (nx == 1)
{
// single item in x range.
// If it's in the yrange, mark its position in the LCS
xs_in_lcs[distance(xo, xlo)] = find(ylo, yhi, *xlo) != yhi;
}
else
{
// split the xrange
it xmid = xlo + nx / 2;
// Find LCS lengths at xmid, working from both ends of the range
lengths ll_b, ll_e;
std::reverse_iterator<it> hix(xhi), midx(xmid), hiy(yhi), loy(ylo);
lcs_lens(xlo, xmid, ylo, yhi, ll_b);
lcs_lens(hix, midx, hiy, loy, ll_e);
// Find the optimal place to split the y range
lengths::const_reverse_iterator e = ll_e.rbegin();
int lmax = -1;
it y = ylo, ymid = ylo;
for (lengths::const_iterator b = ll_b.begin();
b != ll_b.end(); ++y, ++b, ++e)
{
if (*b + *e > lmax)
{
lmax = *b + *e;
ymid = y;
}
}
// Split the range and recurse
calculate_lcs(xo, xlo, xmid, ylo, ymid, xs_in_lcs);
calculate_lcs(xo, xmid, xhi, ymid, yhi, xs_in_lcs);
}
}
Matrix *read_matrix_uci(FILE *fin)
{
if (!fin) {
return NULL;
}
unsigned rows = 0, cols = 0, nnz = 0;
fscanf(fin, "%u%u%u", &cols, &rows, &nnz);
Matrix *matrix = make_matrix_zero(rows, cols);
if (!matrix) {
return NULL;
}
unsigned i = 0, j = 0, line;
double val = 0;
for (line = 0; line < nnz; ++line) {
fscanf(fin ,"%u%u%lf", &j, &i, &val);
matrix->data[midx(matrix, i - 1, j - 1)] = val;
}
return matrix;
}
Matrix *read_matrix(FILE *fin)
{
if (!fin) {
return NULL;
}
unsigned rows = 0, cols = 0;
fscanf(fin, "%u%u", &cols, &rows);
Matrix *matrix = make_matrix_zero(rows, cols);
if (!matrix) {
return NULL;
}
unsigned i, j;
double val = 0;
for (j = 0; j < cols; ++j) {
for (i = 0; i < rows; ++i) {
fscanf(fin, "%lf", &val);
matrix->data[midx(matrix, i, j)] = val;
}
}
return matrix;
}
Matrix *submatrix(Matrix *matrix, int offset_r, int offset_c, int rows, int cols)
{
if (!matrix) {
return NULL;
}
if (offset_r + rows > matrix->rows || offset_c + cols > matrix->cols) {
return NULL;
}
Matrix *submatrix = make_matrix_zero(rows, cols);
if (!submatrix) {
return NULL;
}
unsigned i, j, mi, mj;
submatrix->transposed = matrix->transposed;
for (j = 0; j < cols; ++j) {
mj = j + offset_c;
for (i = 0; i < rows; ++i) {
mi = i + offset_r;
submatrix->data[midx(submatrix, i, j)] = get(matrix, mi, mj);
}
}
return submatrix;
}
Matrix *dot(Matrix *A, Matrix *B)
{
if (!A || !B) {
return NULL;
}
unsigned idx;
int Arows = A->rows;
int Acols = A->cols;
int Brows = B->rows;
int Bcols = B->cols;
if (A->transposed) {
Arows = A->cols;
Acols = A->rows;
}
if (B->transposed) {
Brows = B->cols;
Bcols = B->rows;
}
if (Acols != Brows) {
return NULL;
}
Matrix *C = make_matrix_zero(Arows, Bcols);
#pragma omp parallel for
for (idx = 0; idx < Arows * Bcols; ++idx) {
unsigned i, j;
i = idx % Arows;
j = idx / Arows;
double sum = 0.0;
unsigned k;
for (k = 0; k < Acols; ++k) {
sum += get(A, i, k) * get(B, k, j);
}
C->data[midx(C, i, j)] = sum;
}
return C;
}
DWORD WINAPI LogCallback(LPVOID lpparam)
{
/*
Callback function for reading data from log file.
INPUTS:
lpparam - a pointer to the LogSensor that owns this reader thread.
OUTPUTS:
none. (always returns 0)
*/
//cast the input argument to an instance of the sensor container class
SceneEstimatorLogSensor *SensorLogs = (SceneEstimatorLogSensor *)lpparam;
errno_t retcode;
//this will be used to create each new event as it arrives
Event* NewEvent = NULL;
Event* NewEvent2 = NULL;
//access to the posterior pose event queue
extern SynchronizedEventQueue* ThePosteriorPoseEventQueue;
extern SynchronizedEventQueue* TheTrackGeneratorEventQueue;
//access to the posterior pose odometry queue
extern RelativePoseQueue* TheRelativePoseQueue;
//access to the car time
extern CarTime* TheCarTime;
//access to the logging queue
#ifdef SE_PRINTLOGS
extern EventQueue* TheLoggingQueue;
Event* LogEvent;
#endif
int i;
int j;
int k;
//for determining which event happens next
double eventtimes[9];
int enext;
double etime = DBL_MAX;
while (SensorLogs->IsRunning() == true)
{
//create an event each time sensor data is present
//this will store the event data (i.e. the entire packet)
int iEventType = INVALID_EVENT;
//the event's time stamp
double iEventTime = DBL_MAX;
//the number of elements in the event
int ine = 0;
//the number of columns per row of event data
int inc = 0;
int ine2 = 0;
int inc2 = 0;
int ine3 = 0;
int inc3 = 0;
double* iEventData = NULL;
double* iEventData2 = NULL;
double* iEventData3 = NULL;
//find which event happens next
eventtimes[0] = SensorLogs->OdomLog.LogTime();
eventtimes[1] = SensorLogs->PoseLog.LogTime();
eventtimes[2] = SensorLogs->FrontMobileyeRoadLog.LogTime();
eventtimes[3] = SensorLogs->BackMobileyeRoadLog.LogTime();
eventtimes[4] = SensorLogs->FrontJasonRoadLog.LogTime();
eventtimes[5] = SensorLogs->BackJasonRoadLog.LogTime();
eventtimes[6] = SensorLogs->StoplineLog.LogTime();
eventtimes[7] = SensorLogs->LocalMapLog.LogTime();
eventtimes[8] = SensorLogs->LocalPointsLog.LogTime();
enext = -1;
etime = DBL_MAX;
for (i = 0; i < 9; i++)
{
if (eventtimes[i] < etime)
{
enext = i;
etime = eventtimes[i];
}
}
switch (enext)
{
case 0:
{
//odometry packet comes next
//parse out the odometry event information
iEventData = new double[SE_ODOMPACKETSIZE];
retcode = StringToArrayOfDoubles(iEventData, SE_ODOMPACKETSIZE, SensorLogs->OdomLog.LogBuffer());
if (retcode != SE_ODOMPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed odometry log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//for log files, use the odometry to drive the car time
TheCarTime->SetCurrentCarTime(iEventData[0]);
#ifdef SE_PRINTLOGS
//extract the event time
iEventTime = iEventData[0];
iEventType = ODOM_EVENT;
ine = 1;
inc = SE_ODOMPACKETSIZE;
//create a separate logging event with the same data
LogEvent = new Event();
double* iLogData = new double[SE_ODOMPACKETSIZE];
memcpy(iLogData, iEventData, sizeof(double) * inc);
LogEvent->SetEventType(iEventType, iEventTime);
LogEvent->SetEventData(ine, inc, iLogData);
#endif
bool spush = TheRelativePoseQueue->PushPacket(iEventData);
#ifdef SE_PRINTLOGS
//only log the packet if it successfully entered the event queue
if (spush == true)
{
TheLoggingQueue->PushEvent(LogEvent);
}
else
{
delete LogEvent;
}
#endif
//note: now the event gets pushed directly to the relative pose queue
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
}
//done reading the odometry event; read the next line
SensorLogs->OdomLog.GetNextLogLine();
}
break;
case 1:
{
//pose packet comes next
//parse out the odometry event information
iEventData = new double[SE_POSEPACKETSIZE];
retcode = StringToArrayOfDoubles(iEventData, SE_POSEPACKETSIZE, SensorLogs->PoseLog.LogBuffer());
if (retcode != SE_POSEPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed pose log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = iEventData[0];
iEventType = POSE_EVENT;
ine = 1;
inc = SE_POSEPACKETSIZE;
}
//done reading the event; read the next line
SensorLogs->PoseLog.GetNextLogLine();
}
break;
case 2:
{
//front mobileye packet comes next
//parse out the mobileye event information
iEventData = new double[SE_MOBILEYEROADPACKETSIZE];
retcode = StringToArrayOfDoubles(iEventData, SE_MOBILEYEROADPACKETSIZE, SensorLogs->FrontMobileyeRoadLog.LogBuffer());
if (retcode != SE_MOBILEYEROADPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed front mobileye log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = iEventData[0];
iEventType = FRONTMOBILEYEROAD_EVENT;
ine = 1;
inc = SE_MOBILEYEROADPACKETSIZE;
}
//done reading the event; read the next line
SensorLogs->FrontMobileyeRoadLog.GetNextLogLine();
}
break;
case 3:
{
//back mobileye packet comes next
//parse out the mobileye event information
iEventData = new double[SE_MOBILEYEROADPACKETSIZE];
retcode = StringToArrayOfDoubles(iEventData, SE_MOBILEYEROADPACKETSIZE, SensorLogs->BackMobileyeRoadLog.LogBuffer());
if (retcode != SE_MOBILEYEROADPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed back mobileye log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = iEventData[0];
iEventType = BACKMOBILEYEROAD_EVENT;
ine = 1;
inc = SE_MOBILEYEROADPACKETSIZE;
}
//done reading the event; read the next line
SensorLogs->BackMobileyeRoadLog.GetNextLogLine();
}
break;
case 4:
{
//front jason packet comes next
//parse out the jason event information
double tEventData[SE_JASONROADPACKETSIZE];
retcode = StringToArrayOfDoubles(tEventData, SE_JASONROADPACKETSIZE, SensorLogs->FrontJasonRoadLog.LogBuffer());
if (retcode != SE_JASONROADPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed front jason log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
iEventType = FRONTJASONROAD_EVENT;
ine = (int) (tEventData[2]);
inc = SE_JASONROADPACKETSIZE;
iEventData = new double[inc*ine];
for (j = 0; j < inc; j++)
{
//copy the first line of the packet into iEventData
iEventData[midx(0, j, ine)] = tEventData[j];
}
for (i = 1; i < ine; i++)
{
//read the rest of the packet line by line
SensorLogs->FrontJasonRoadLog.GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, SE_JASONROADPACKETSIZE, SensorLogs->FrontJasonRoadLog.LogBuffer());
if (retcode != SE_JASONROADPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed front jason log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
break;
}
for (j = 0; j < SE_JASONROADPACKETSIZE; j++)
{
//copy the next line of the packet into iEventData
iEventData[midx(i, j, ine)] = tEventData[j];
}
}
}
//done reading the event; read the next line
SensorLogs->FrontJasonRoadLog.GetNextLogLine();
}
break;
case 5:
{
//back jason packet comes next
//parse out the jason event information
double tEventData[SE_JASONROADPACKETSIZE];
retcode = StringToArrayOfDoubles(tEventData, SE_JASONROADPACKETSIZE, SensorLogs->BackJasonRoadLog.LogBuffer());
if (retcode != SE_JASONROADPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed back jason log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
iEventType = BACKJASONROAD_EVENT;
ine = (int) (tEventData[2]);
inc = SE_JASONROADPACKETSIZE;
iEventData = new double[inc*ine];
for (j = 0; j < inc; j++)
{
//copy the first line of the packet into iEventData
iEventData[midx(0, j, ine)] = tEventData[j];
}
for (i = 1; i < ine; i++)
{
//read the rest of the packet line by line
SensorLogs->FrontJasonRoadLog.GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, SE_JASONROADPACKETSIZE, SensorLogs->BackJasonRoadLog.LogBuffer());
if (retcode != SE_JASONROADPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed back jason log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
break;
}
for (j = 0; j < SE_JASONROADPACKETSIZE; j++)
{
//copy the next line of the packet into iEventData
iEventData[midx(i, j, ine)] = tEventData[j];
}
}
}
//done reading the event; read the next line
SensorLogs->BackJasonRoadLog.GetNextLogLine();
}
break;
case 6:
{
//stopline packet comes next
//parse out the stopline event information
double tEventData[SE_STOPLINEPACKETSIZE];
retcode = StringToArrayOfDoubles(tEventData, SE_STOPLINEPACKETSIZE, SensorLogs->StoplineLog.LogBuffer());
if (retcode != SE_STOPLINEPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed stopline log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
iEventType = STOPLINE_EVENT;
ine = (int) (tEventData[2]);
inc = SE_STOPLINEPACKETSIZE;
iEventData = new double[inc*ine];
for (j = 0; j < SE_STOPLINEPACKETSIZE; j++)
{
//copy the first line of the packet into iEventData
iEventData[midx(0, j, ine)] = tEventData[j];
}
for (i = 1; i < ine; i++)
{
//read the rest of the packet line by line
SensorLogs->StoplineLog.GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, SE_STOPLINEPACKETSIZE, SensorLogs->StoplineLog.LogBuffer());
if (retcode != SE_STOPLINEPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed stopline log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
break;
}
for (j = 0; j < inc; j++)
{
//copy the next line of the packet into iEventData
iEventData[midx(i, j, ine)] = tEventData[j];
}
}
}
//done reading the event; read the next line
SensorLogs->StoplineLog.GetNextLogLine();
}
break;
case 7:
{
//local map packet comes next
//parse out the localmap event information
double tEventData[SE_LMTHEADERPACKETSIZE];
double tEventData2[SE_LMTCOVARIANCEPACKETSIZE];
double tEventData3[SE_LMTPOINTSPACKETSIZE];
//parse the first target header
retcode = StringToArrayOfDoubles(tEventData, SE_LMTHEADERPACKETSIZE, SensorLogs->LocalMapLog.LogBuffer());
if (retcode != SE_LMTHEADERPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed localmap target log packet.\n");
//try to advance the log
SensorLogs->LocalMapLog.GetNextLogLine();
}
else
{
//if code gets here, tEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
iEventType = LOCALMAPTARGETS_EVENT;
//extract the number of targets and store target headers in iEventData
ine = (int) (tEventData[2]);
inc = SE_LMTHEADERPACKETSIZE;
iEventData = new double[inc*ine];
ine2 = ine;
inc2 = SE_LMTCOVARIANCEPACKETSIZE;
iEventData2 = new double[ine2*inc2];
ine3 = 0;
inc3 = SE_LMTPOINTSPACKETSIZE;
//create temporary space to hold all the points
int tidx = 0;
double tSpace[LS_MAXTARGETPOINTS*SE_LMTPOINTSPACKETSIZE];
for (i = 0; i < ine; i++)
{
//read in each target
//1. copy the target header into iEventData
if (retcode != SE_LMTHEADERPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed localmap target log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
ine2 = 0;
inc2 = 0;
delete [] iEventData2;
iEventData2 = NULL;
ine3 = 0;
inc3 = 0;
delete [] iEventData3;
iEventData3 = NULL;
break;
}
for (j = 0; j < SE_LMTHEADERPACKETSIZE; j++)
{
iEventData[midx(i, j, ine)] = tEventData[j];
}
//pull the number of points for this target
int np = (int) tEventData[10];
//2. read the covariance packet for this target
SensorLogs->LocalMapLog.GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData2, SE_LMTCOVARIANCEPACKETSIZE, SensorLogs->LocalMapLog.LogBuffer());
if (retcode != SE_LMTCOVARIANCEPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed localmap target log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
ine2 = 0;
inc2 = 0;
delete [] iEventData2;
iEventData2 = NULL;
ine3 = 0;
inc3 = 0;
delete [] iEventData3;
iEventData3 = NULL;
break;
}
//copy the target covariance into iEventData2
for (j = 0; j < inc2; j++)
{
iEventData2[midx(i, j, ine)] = tEventData2[j];
}
//3. read each points packet for this target
bool pSuccess = true;
if (np > 0)
{
for (j = 0; j < np; j++)
{
SensorLogs->LocalMapLog.GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData3, SE_LMTPOINTSPACKETSIZE, SensorLogs->LocalMapLog.LogBuffer());
if (retcode != SE_LMTPOINTSPACKETSIZE)
{
//improper packet read: throw out this packet
pSuccess = false;
break;
}
//copy the point into tSpace
for (k = 0; k < inc3; k++)
{
tSpace[midx(tidx, k, LS_MAXTARGETPOINTS)] = tEventData3[k];
}
tidx++;
}
}
if (pSuccess == false)
{
//improper packet read: throw out this packet
printf("Malformed localmap target log packet.\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
ine2 = 0;
inc2 = 0;
delete [] iEventData2;
iEventData2 = NULL;
ine3 = 0;
inc3 = 0;
delete [] iEventData3;
iEventData3 = NULL;
//try to advance the log
SensorLogs->LocalMapLog.GetNextLogLine();
break;
}
//done parsing one target, move to the next
SensorLogs->LocalMapLog.GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, SE_LMTHEADERPACKETSIZE, SensorLogs->LocalMapLog.LogBuffer());
}
//done parsing all targets, now set the event data
ine3 = tidx;
if (ine3 > 0)
{
//if there are any points, allocate memory and copy them into iEventData3
iEventData3 = new double[ine3*inc3];
for (i = 0; i < ine3; i++)
{
for (j = 0; j < inc3; j++)
{
iEventData3[midx(i, j, ine3)] = tSpace[midx(i, j, LS_MAXTARGETPOINTS)];
}
}
}
//all the targets have been processed
//NOTE: the next target header is already loaded
}
}
break;
case 8:
{
//local points packet comes next
//parse out the localmap event information
double tEventData[SE_LOCALPOINTSPACKETSIZE];
retcode = StringToArrayOfDoubles(tEventData, SE_LOCALPOINTSPACKETSIZE, SensorLogs->LocalPointsLog.LogBuffer());
if (retcode != SE_LOCALPOINTSPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed local points log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
iEventType = LOCALMAPPOINTS_EVENT;
ine = (int) (tEventData[2]);
inc = SE_LOCALPOINTSPACKETSIZE;
iEventData = new double[inc*ine];
for (j = 0; j < inc; j++)
{
//copy the first line of the packet into iEventData
iEventData[midx(0, j, ine)] = tEventData[j];
}
for (i = 1; i < ine; i++)
{
//read the rest of the packet line by line
SensorLogs->LocalPointsLog.GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, SE_LOCALPOINTSPACKETSIZE, SensorLogs->LocalPointsLog.LogBuffer());
if (retcode != SE_LOCALPOINTSPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed local points log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
delete [] iEventData;
iEventData = NULL;
break;
}
for (j = 0; j < inc; j++)
{
//copy the next line of the packet into iEventData
iEventData[midx(i, j, ine)] = tEventData[j];
}
}
}
//done reading the event; read the next line
SensorLogs->LocalPointsLog.GetNextLogLine();
}
break;
default:
{
//invalid event
iEventType = INVALID_EVENT;
iEventTime = DBL_MAX;
ine = 0;
inc = 0;
iEventData = NULL;
}
break;
}
//when no data is left, "disconnect" the sensor
if (etime == DBL_MAX)
{
printf("Log sensors terminating...\n");
iEventType = INVALID_EVENT;
iEventTime = DBL_MAX;
ine = 0;
inc = 0;
iEventData = NULL;
SensorLogs->StopRunning();
}
if (iEventType == INVALID_EVENT || SensorLogs->IsRunning() == false)
{
//for invalid reads or terminating logfiles, go to sleep
//note: memory has already been deallocated for bad reads
//sleep for the delay time of the sensor
Sleep(SensorLogs->SamplePeriod());
continue;
}
//when the code gets here, iEventData is populated with a complete sensor packet
//create the event data
NewEvent = new Event();
NewEvent->SetEventType(iEventType, iEventTime);
NewEvent->SetEventData(ine, inc, iEventData);
NewEvent->SetEventData2(ine2, inc2, iEventData2);
NewEvent->SetEventData3(ine3, inc3, iEventData3);
#ifdef SE_PRINTLOGS
//create a separate logging event with the same data
LogEvent = new Event(NewEvent);
#endif
bool spush = false;
switch (NewEvent->EventType())
{
case ODOM_EVENT:
case POSE_EVENT:
case FRONTMOBILEYEROAD_EVENT:
case BACKMOBILEYEROAD_EVENT:
case FRONTJASONROAD_EVENT:
case BACKJASONROAD_EVENT:
//positioning packets go to PosteriorPose
spush = ThePosteriorPoseEventQueue->PushEvent(NewEvent);
break;
case LOCALMAPTARGETS_EVENT:
case LOCALMAPPOINTS_EVENT:
//localmap packets go to track generator
spush = TheTrackGeneratorEventQueue->PushEvent(NewEvent);
break;
case QUIT_EVENT:
case TRANSMIT_EVENT:
//message events go to both queues
NewEvent2 = new Event(NewEvent);
spush = ThePosteriorPoseEventQueue->PushEvent(NewEvent);
TheTrackGeneratorEventQueue->PushEvent(NewEvent2);
break;
}
#ifdef SE_PRINTLOGS
//only log the packet if it successfully entered the event queue
if (spush == true)
{
TheLoggingQueue->PushEvent(LogEvent);
}
else
{
delete LogEvent;
}
#endif
//sleep for the delay time of the sensor
Sleep(SensorLogs->SamplePeriod());
}
return 0;
}
DWORD WINAPI LogCallback(LPVOID lpparam)
{
/*
Callback function for reading data from log file.
INPUTS:
lpparam - a pointer to the LogSensor that owns this reader thread.
OUTPUTS:
none. (always returns 0)
*/
//cast the input argument to an instance of the sensor container class
LocalMapLogSensor *SensorLogs = (LocalMapLogSensor *)lpparam;
errno_t retcode;
//this will store the event data (i.e. the entire packet)
int iEventType = INVALID_EVENT;
//the event's time stamp
double iEventTime = DBL_MAX;
//the number of elements in the event
int ine = 0;
//the number of columns per row of event data
int inc = 0;
double *iEventData = NULL;
//this will be used to create each new event as it arrives
Event* NewEvent = NULL;
Event* NewEvent2 = NULL;
//access to the event queues
extern SynchronizedEventQueue* TheLocalMapEventQueue;
extern SynchronizedEventQueue* TheLocalRoadEventQueue;
//access to the odometry queue
extern RelativePoseQueue* TheRelativePoseQueue;
//access to the car time
extern CarTime* TheCarTime;
//access to the logging queue
#ifdef LM_PRINTLOGS
extern EventQueue* TheLoggingQueue;
Event* LogEvent;
#endif
int i;
int j;
//for determining which event happens next
double eventtimes[20];
int enext;
double etime = DBL_MAX;
ReaderLogFile* TheReaderLogFile;
while (SensorLogs->IsRunning() == true)
{
//create an event each time sensor data is present
//find which event happens next
eventtimes[0] = SensorLogs->OdomLog.LogTime();
eventtimes[1] = SensorLogs->FrontMobileyeObstacleLog.LogTime();
eventtimes[2] = SensorLogs->FrontMobileyeRoadLog.LogTime();
eventtimes[3] = SensorLogs->BackMobileyeObstacleLog.LogTime();
eventtimes[4] = SensorLogs->BackMobileyeRoadLog.LogTime();
eventtimes[5] = SensorLogs->FrontJasonRoadLog.LogTime();
eventtimes[6] = SensorLogs->BackJasonRoadLog.LogTime();
eventtimes[7] = SensorLogs->IbeoLog.LogTime();
eventtimes[8] = SensorLogs->BackClusteredSickLog.LogTime();
eventtimes[9] = SensorLogs->Front0RadarLog.LogTime();
eventtimes[10] = SensorLogs->Driv0RadarLog.LogTime();
eventtimes[11] = SensorLogs->Driv1RadarLog.LogTime();
eventtimes[12] = SensorLogs->Driv2RadarLog.LogTime();
eventtimes[13] = SensorLogs->Driv3RadarLog.LogTime();
eventtimes[14] = SensorLogs->Pass0RadarLog.LogTime();
eventtimes[15] = SensorLogs->Pass1RadarLog.LogTime();
eventtimes[16] = SensorLogs->Pass2RadarLog.LogTime();
eventtimes[17] = SensorLogs->Pass3RadarLog.LogTime();
eventtimes[18] = SensorLogs->DrivSideSickLog.LogTime();
eventtimes[19] = SensorLogs->PassSideSickLog.LogTime();
enext = -1;
etime = DBL_MAX;
for (i = 0; i < 20; i++)
{
if (eventtimes[i] < etime)
{
enext = i;
etime = eventtimes[i];
}
}
switch (enext)
{
case 0:
{
//odometry packet comes next
//parse out the odometry event information
iEventData = new double[LM_ODOMPACKETSIZE];
retcode = StringToArrayOfDoubles(iEventData, LM_ODOMPACKETSIZE, SensorLogs->OdomLog.LogBuffer());
if (retcode != LM_ODOMPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed odometry log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//for log files, use the odometry to drive the car time
TheCarTime->SetCurrentCarTime(iEventData[0]);
#ifdef LM_PRINTLOGS
//extract the event time
iEventTime = iEventData[0];
iEventType = ODOM_EVENT;
ine = 1;
inc = LM_ODOMPACKETSIZE;
//create a separate logging event with the same data
LogEvent = new Event();
double* iLogData = new double[LM_ODOMPACKETSIZE];
memcpy(iLogData, iEventData, sizeof(double) * inc);
LogEvent->SetEventType(iEventType, iEventTime);
LogEvent->SetEventData(ine, inc, iLogData);
#endif
bool spush = TheRelativePoseQueue->PushPacket(iEventData);
#ifdef LM_PRINTLOGS
//only log the packet if it successfully entered the event queue
if (spush == true)
{
TheLoggingQueue->PushEvent(LogEvent);
}
else
{
delete LogEvent;
}
#endif
//note: now the event gets pushed directly to the relative pose queue
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
}
//done reading the odometry event; read the next line
SensorLogs->OdomLog.GetNextLogLine();
}
break;
case 1:
case 3:
{
//front / back mobileye obstacle packet comes next
switch(enext)
{
case 1:
iEventType = FRONTMOBILEYEOBSTACLE_EVENT;
TheReaderLogFile = &(SensorLogs->FrontMobileyeObstacleLog);
break;
case 3:
iEventType = BACKMOBILEYEOBSTACLE_EVENT;
TheReaderLogFile = &(SensorLogs->BackMobileyeObstacleLog);
break;
}
//parse out the mobileye event information
double tEventData[LM_MOBILEYEOBSTACLEPACKETSIZE];
retcode = StringToArrayOfDoubles(tEventData, LM_MOBILEYEOBSTACLEPACKETSIZE, TheReaderLogFile->LogBuffer());
if (retcode != LM_MOBILEYEOBSTACLEPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed mobileye obstacle log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
ine = (int) (tEventData[2]);
inc = LM_MOBILEYEOBSTACLEPACKETSIZE;
iEventData = new double[inc*ine];
for (j = 0; j < inc; j++)
{
//copy the first line of the packet into iEventData
iEventData[midx(0, j, ine)] = tEventData[j];
}
for (i = 1; i < ine; i++)
{
//read the rest of the packet line by line
TheReaderLogFile->GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, LM_MOBILEYEOBSTACLEPACKETSIZE, TheReaderLogFile->LogBuffer());
if (retcode != LM_MOBILEYEOBSTACLEPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed mobileye obstacle log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
break;
}
for (j = 0; j < LM_MOBILEYEOBSTACLEPACKETSIZE; j++)
{
//copy the next line of the packet into iEventData
iEventData[midx(i, j, ine)] = tEventData[j];
}
}
}
//done reading the event; read the next line
TheReaderLogFile->GetNextLogLine();
}
break;
case 2:
case 4:
{
//front / back mobileye road packet comes next
switch(enext)
{
case 2:
iEventType = FRONTMOBILEYEROAD_EVENT;
TheReaderLogFile = &(SensorLogs->FrontMobileyeRoadLog);
break;
case 4:
iEventType = BACKMOBILEYEROAD_EVENT;
TheReaderLogFile = &(SensorLogs->BackMobileyeRoadLog);
break;
}
//parse out the mobileye event information
iEventData = new double[LM_MOBILEYEROADPACKETSIZE];
retcode = StringToArrayOfDoubles(iEventData, LM_MOBILEYEROADPACKETSIZE, TheReaderLogFile->LogBuffer());
if (retcode != LM_MOBILEYEROADPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed mobileye road log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = iEventData[0];
ine = 1;
inc = LM_MOBILEYEROADPACKETSIZE;
}
//done reading the event; read the next line
TheReaderLogFile->GetNextLogLine();
}
break;
case 5:
case 6:
{
//front / back jason packet comes next
switch(enext)
{
case 5:
iEventType = FRONTJASONROAD_EVENT;
TheReaderLogFile = &(SensorLogs->FrontJasonRoadLog);
break;
case 6:
iEventType = BACKJASONROAD_EVENT;
TheReaderLogFile = &(SensorLogs->BackJasonRoadLog);
break;
}
//parse out the jason event information
double tEventData[LM_JASONROADPACKETSIZE];
retcode = StringToArrayOfDoubles(tEventData, LM_JASONROADPACKETSIZE, TheReaderLogFile->LogBuffer());
if (retcode != LM_JASONROADPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed jason road log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
ine = (int) (tEventData[2]);
inc = LM_JASONROADPACKETSIZE;
iEventData = new double[inc*ine];
for (j = 0; j < inc; j++)
{
//copy the first line of the packet into iEventData
iEventData[midx(0, j, ine)] = tEventData[j];
}
for (i = 1; i < ine; i++)
{
//read the rest of the packet line by line
SensorLogs->FrontJasonRoadLog.GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, LM_JASONROADPACKETSIZE, TheReaderLogFile->LogBuffer());
if (retcode != LM_JASONROADPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed jason road log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
break;
}
for (j = 0; j < LM_JASONROADPACKETSIZE; j++)
{
//copy the next line of the packet into iEventData
iEventData[midx(i, j, ine)] = tEventData[j];
}
}
}
//done reading the event; read the next line
TheReaderLogFile->GetNextLogLine();
}
break;
case 7:
{
//front ibeo packet comes next
//parse out the front ibeo event information
double tEventData[LM_CLUSTEREDIBEOPACKETSIZE];
retcode = StringToArrayOfDoubles(tEventData, LM_CLUSTEREDIBEOPACKETSIZE, SensorLogs->IbeoLog.LogBuffer());
if (retcode != LM_CLUSTEREDIBEOPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed front ibeo log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
iEventType = IBEO_EVENT;
ine = (int) (tEventData[2]);
inc = LM_CLUSTEREDIBEOPACKETSIZE;
iEventData = new double[inc*ine];
for (j = 0; j < LM_CLUSTEREDIBEOPACKETSIZE; j++)
{
//copy the first line of the packet into iEventData
iEventData[midx(0, j, ine)] = tEventData[j];
}
for (i = 1; i < ine; i++)
{
//read the rest of the packet line by line
SensorLogs->IbeoLog.GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, LM_CLUSTEREDIBEOPACKETSIZE, SensorLogs->IbeoLog.LogBuffer());
if (retcode != LM_CLUSTEREDIBEOPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed front ibeo log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
break;
}
for (j = 0; j < inc; j++)
{
//copy the next line of the packet into iEventData
iEventData[midx(i, j, ine)] = tEventData[j];
}
}
}
//done reading the event; read the next line
SensorLogs->IbeoLog.GetNextLogLine();
}
break;
case 8:
{
//back clustered sick packet comes next
//parse out the front ibeo event information
double tEventData[LM_CLUSTEREDSICKPACKETSIZE];
retcode = StringToArrayOfDoubles(tEventData, LM_CLUSTEREDSICKPACKETSIZE, SensorLogs->BackClusteredSickLog.LogBuffer());
if (retcode != LM_CLUSTEREDSICKPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed back clustered sick log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
iEventType = BACKCLUSTEREDSICK_EVENT;
ine = (int) (tEventData[2]);
inc = LM_CLUSTEREDSICKPACKETSIZE;
iEventData = new double[inc*ine];
for (j = 0; j < LM_CLUSTEREDSICKPACKETSIZE; j++)
{
//copy the first line of the packet into iEventData
iEventData[midx(0, j, ine)] = tEventData[j];
}
for (i = 1; i < ine; i++)
{
//read the rest of the packet line by line
SensorLogs->BackClusteredSickLog.GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, LM_CLUSTEREDSICKPACKETSIZE, SensorLogs->BackClusteredSickLog.LogBuffer());
if (retcode != LM_CLUSTEREDSICKPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed back clustered sick log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
break;
}
for (j = 0; j < inc; j++)
{
//copy the next line of the packet into iEventData
iEventData[midx(i, j, ine)] = tEventData[j];
}
}
}
//done reading the event; read the next line
SensorLogs->BackClusteredSickLog.GetNextLogLine();
}
break;
case 9:
case 10:
case 11:
case 12:
case 13:
case 14:
case 15:
case 16:
case 17:
switch(enext)
{
case 9:
iEventType = FRONT0RADAR_EVENT;
TheReaderLogFile = &(SensorLogs->Front0RadarLog);
break;
case 10:
iEventType = DRIV0RADAR_EVENT;
TheReaderLogFile = &(SensorLogs->Driv0RadarLog);
break;
case 11:
iEventType = DRIV1RADAR_EVENT;
TheReaderLogFile = &(SensorLogs->Driv1RadarLog);
break;
case 12:
iEventType = DRIV2RADAR_EVENT;
TheReaderLogFile = &(SensorLogs->Driv2RadarLog);
break;
case 13:
iEventType = DRIV3RADAR_EVENT;
TheReaderLogFile = &(SensorLogs->Driv3RadarLog);
break;
case 14:
iEventType = PASS0RADAR_EVENT;
TheReaderLogFile = &(SensorLogs->Pass0RadarLog);
break;
case 15:
iEventType = PASS1RADAR_EVENT;
TheReaderLogFile = &(SensorLogs->Pass1RadarLog);
break;
case 16:
iEventType = PASS2RADAR_EVENT;
TheReaderLogFile = &(SensorLogs->Pass2RadarLog);
break;
case 17:
iEventType = PASS3RADAR_EVENT;
TheReaderLogFile = &(SensorLogs->Pass3RadarLog);
break;
}
{
//radar packet comes next
//parse out the radar event information
double tEventData[LM_DELPHIPACKETSIZE];
retcode = StringToArrayOfDoubles(tEventData, LM_DELPHIPACKETSIZE, TheReaderLogFile->LogBuffer());
if (retcode != LM_DELPHIPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed radar log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
//NOTE: iEventType is already set in the cascading cases
ine = (int) (tEventData[2]);
inc = LM_DELPHIPACKETSIZE;
iEventData = new double[inc*ine];
for (j = 0; j < LM_DELPHIPACKETSIZE; j++)
{
//copy the first line of the packet into iEventData
iEventData[midx(0, j, ine)] = tEventData[j];
}
for (i = 1; i < ine; i++)
{
//read the rest of the packet line by line
TheReaderLogFile->GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, LM_DELPHIPACKETSIZE, TheReaderLogFile->LogBuffer());
if (retcode != LM_DELPHIPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed radar log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
break;
}
for (j = 0; j < inc; j++)
{
//copy the next line of the packet into iEventData
iEventData[midx(i, j, ine)] = tEventData[j];
}
}
}
//done reading the event; read the next line
TheReaderLogFile->GetNextLogLine();
}
break;
case 18:
case 19:
{
//driv / pass side sick event came next
switch(enext)
{
case 18:
iEventType = SIDESICKDRIV_EVENT;
TheReaderLogFile = &(SensorLogs->DrivSideSickLog);
break;
case 19:
iEventType = SIDESICKPASS_EVENT;
TheReaderLogFile = &(SensorLogs->PassSideSickLog);
break;
}
//parse out the side sick event information
double tEventData[LM_SIDESICKPACKETSIZE];
retcode = StringToArrayOfDoubles(tEventData, LM_SIDESICKPACKETSIZE, TheReaderLogFile->LogBuffer());
if (retcode != LM_SIDESICKPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed side sick log packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
iEventData = NULL;
}
else
{
//if code gets here, iEventData was created successfully
//extract the event time
iEventTime = tEventData[0];
//NOTE: iEventType is already set in the cascading cases
ine = (int) (tEventData[2]);
inc = LM_SIDESICKPACKETSIZE;
iEventData = new double[inc*ine];
for (j = 0; j < LM_SIDESICKPACKETSIZE; j++)
{
//copy the first line of the packet into iEventData
iEventData[midx(0, j, ine)] = tEventData[j];
}
for (i = 1; i < ine; i++)
{
//read the rest of the packet line by line
TheReaderLogFile->GetNextLogLine();
retcode = StringToArrayOfDoubles(tEventData, LM_SIDESICKPACKETSIZE, TheReaderLogFile->LogBuffer());
if (retcode != LM_SIDESICKPACKETSIZE)
{
//improper packet read: throw out this packet
printf("Malformed side sick packet\n");
iEventTime = DBL_MAX;
iEventType = INVALID_EVENT;
ine = 0;
inc = 0;
delete [] iEventData;
iEventData = NULL;
break;
}
for (j = 0; j < inc; j++)
{
//copy the next line of the packet into iEventData
iEventData[midx(i, j, ine)] = tEventData[j];
}
}
}
//done reading the event; read the next line
TheReaderLogFile->GetNextLogLine();
}
break;
default:
{
//invalid event
iEventType = INVALID_EVENT;
iEventTime = DBL_MAX;
ine = 0;
inc = 0;
iEventData = NULL;
}
break;
}
//when no data is left, "disconnect" the sensor
if (etime == DBL_MAX)
{
printf("Log sensors terminating...\n");
iEventType = INVALID_EVENT;
iEventTime = DBL_MAX;
ine = 0;
inc = 0;
iEventData = NULL;
SensorLogs->StopRunning();
}
if (iEventType == INVALID_EVENT || SensorLogs->IsRunning() == false)
{
//for invalid reads or terminating logfiles, go to sleep
//note: memory has already been deallocated for bad reads
//sleep for the delay time of the sensor
Sleep(SensorLogs->SamplePeriod());
continue;
}
//when the code gets here, iEventData is populated with a complete sensor packet
//create the event data
NewEvent = new Event();
NewEvent->SetEventType(iEventType, iEventTime);
NewEvent->SetEventData(ine, inc, iEventData);
#ifdef LM_PRINTLOGS
//create a separate logging event with the same data
LogEvent = new Event(NewEvent);
#endif
bool spush = false;
switch (NewEvent->EventType())
{
case IBEO_EVENT:
case BACKCLUSTEREDSICK_EVENT:
case FRONTMOBILEYEOBSTACLE_EVENT:
case BACKMOBILEYEOBSTACLE_EVENT:
case DRIV3RADAR_EVENT:
case DRIV2RADAR_EVENT:
case DRIV1RADAR_EVENT:
case DRIV0RADAR_EVENT:
case FRONT0RADAR_EVENT:
case PASS0RADAR_EVENT:
case PASS1RADAR_EVENT:
case PASS2RADAR_EVENT:
case PASS3RADAR_EVENT:
case SIDESICKDRIV_EVENT:
case SIDESICKPASS_EVENT:
//obstacle packets go to LocalMap
spush = TheLocalMapEventQueue->PushEvent(NewEvent);
break;
case FRONTJASONROAD_EVENT:
case BACKJASONROAD_EVENT:
case FRONTMOBILEYEROAD_EVENT:
case BACKMOBILEYEROAD_EVENT:
//local road packets go to LocalRoad
spush = TheLocalRoadEventQueue->PushEvent(NewEvent);
break;
case ODOM_EVENT:
case QUIT_EVENT:
case TRANSMIT_EVENT:
//message events go to both queues
NewEvent2 = new Event(NewEvent);
spush = TheLocalMapEventQueue->PushEvent(NewEvent);
TheLocalRoadEventQueue->PushEvent(NewEvent2);
break;
}
#ifdef LM_PRINTLOGS
//only log the packet if it successfully entered the event queue
if (spush == true)
{
TheLoggingQueue->PushEvent(LogEvent);
}
else
{
delete LogEvent;
}
#endif
//sleep for the delay time of the sensor
Sleep(SensorLogs->SamplePeriod());
}
return 0;
}
void TargetOccupancyGrid::SetOccupancyGrid(int iNumPoints, double* iPoints,
double iAnchorX, double iAnchorY, double iOrientation)
{
/*
Sets the occupancy grid for a set of target points.
INPUTS:
iNumPoints - number of cluster points for the target
iPoints - the actual target points, in object storage frame
iAnchorX, iAnchorY - the target's anchor point, in ego-vehicle
coordinates
iOrientation - the target's orientation angle
OUTPUTS:
none.
*/
int i;
mIsValid = false;
ResetOccupancyGrid();
if (iNumPoints < 1)
{
//can't create an occupancy grid without points
return;
}
double cosOrient = cos(iOrientation);
double sinOrient = sin(iOrientation);
//store the points in ego-vehicle coordinates
mNumPoints = iNumPoints;
mOccupancyPoints.resize(iNumPoints);
double wt = 1.0 / ((double) iNumPoints);
//these will be built up as the points are transformed to ego-vehicle
mCenterX = 0.0;
mCenterY = 0.0;
double mMinX = DBL_MAX;
double mMaxX = -DBL_MAX;
double mMinY = DBL_MAX;
double mMaxY = -DBL_MAX;
//convert all points to ego-vehicle coordinates
for (i = 0; i < iNumPoints; i++)
{
//extract each target point in object storage frme
double osx = iPoints[midx(i, 0, iNumPoints)];
double osy = iPoints[midx(i, 1, iNumPoints)];
//convert to ego vehicle coordinates
double evx;
double evy;
ObjectToEgoVehicle(evx, evy, osx, osy, cosOrient, sinOrient, iAnchorX, iAnchorY);
//build up the center of mass as the grid center
mCenterX += wt*evx;
mCenterY += wt*evy;
//keep track of the bounding box
if (evx < mMinX)
{
mMinX = evx;
}
if (evx > mMaxX)
{
mMaxX = evx;
}
if (evy < mMinY)
{
mMinY = evy;
}
if (evy > mMaxY)
{
mMaxY = evy;
}
//store the ego-vehicle transformations for later
mOccupancyPoints[i] = make_pair(evx, evy);
}
//calculate the span of the occupancy grid
mSpanX = fabs(mMaxX - mMinX) + 2.0*TOG_PADSIZE;
mSpanY = fabs(mMaxY - mMinY) + 2.0*TOG_PADSIZE;
//calculate the cell lengths of the occupancy grid
mCellLengthX = mSpanX / ((double) mNumCols);
mCellLengthY = mSpanY / ((double) mNumRows);
//populate the occupancy grid
int nc = mNumRows*mNumCols;
for (i = 0; i < iNumPoints; i++)
{
//pull each point
DoublePair curpt = mOccupancyPoints[i];
//and add it to the occupancy grid
int r;
int c;
OccupancyGridCellRowColumn(r, c, curpt.first, curpt.second);
//mark this cell as occupied
if (r >= 0 && r < mNumRows && c >= 0 && c < mNumCols)
{
mOccupancyGrid[OccupancyGridCellIndex(r, c)] = TOG_OCCUPIED;
}
}
//when done populating the grid, it is valid again
mIsValid = true;
return;
}
void LocalRoadCallback(LocalRoadModelEstimateMsg LRMessage, LocalMapInterfaceReceiver* LMRX, void* Arg)
{
/*
Callback for handling the local road model coming from LocalMap
INPUTS:
LRMessage - the actual local road message data
LMRX - the local map interface receiver
Arg - a void pointer (not used)
OUTPUTS:
none. Creates an event and places it on the event queue
*/
//gain access to the event queue
extern SynchronizedEventQueue* ThePosteriorPoseEventQueue;
double iEventTime = LRMessage.timestamp;
//check the event time for valid timestamp
extern CarTime* TheCarTime;
double cct = TheCarTime->CurrentCarTime();
if (fabs(iEventTime - cct) > CT_TIMEJUMPREJECT)
{
//do not allow events with inconsistent timestamps into the event queues
printf("Warning: got localroad timestamp %.12lg; expected something near %.12lg\n", iEventTime, cct);
return;
}
int iNumDataRows = 1;
int iNumDataCols = SE_LRHEADERPACKETSIZE;
//construct the header packet
double* iEventData = new double[SE_LRHEADERPACKETSIZE];
iEventData[0] = LRMessage.timestamp;
iEventData[1] = LRMessage.probabilityRoadModelValid;
iEventData[2] = LRMessage.probabilityLeftLaneExists;
iEventData[3] = LRMessage.laneWidthLeft;
iEventData[4] = LRMessage.laneWidthLeftVariance;
iEventData[5] = LRMessage.probabilityCenterLaneExists;
iEventData[6] = LRMessage.laneWidthCenter;
iEventData[7] = LRMessage.laneWidthCenterVariance;
iEventData[8] = LRMessage.probabilityRightLaneExists;
iEventData[9] = LRMessage.laneWidthRight;
iEventData[10] = LRMessage.laneWidthRightVariance;
iEventData[11] = LRMessage.numPointsLeft;
iEventData[12] = LRMessage.numPointsCenter;
iEventData[13] = LRMessage.numPointsRight;
//construct the points packet
int npL = LRMessage.numPointsLeft;
int npC = LRMessage.numPointsCenter;
int npR = LRMessage.numPointsRight;
int np = npL + npC + npR;
int iNumDataRows2 = np;
int iNumDataCols2 = SE_LRPOINTSPACKETSIZE;
double* iEventData2 = new double[np*SE_LRPOINTSPACKETSIZE];
int i;
int idx = 0;
for (i = 0; i < npL; i++)
{
//set each left lane point in the points packet
iEventData2[midx(idx, 0, np)] = iEventTime;
iEventData2[midx(idx, 1, np)] = (double) i;
iEventData2[midx(idx, 2, np)] = (double) npL;
iEventData2[midx(idx, 3, np)] = 0.0;
iEventData2[midx(idx, 4, np)] = LRMessage.LanePointsLeft[i].GetX();
iEventData2[midx(idx, 5, np)] = LRMessage.LanePointsLeft[i].GetY();
iEventData2[midx(idx, 6, np)] = LRMessage.LanePointsLeft[i].GetVariance();
idx++;
}
for (i = 0; i < npC; i++)
{
//set each center lane point in the points packet
iEventData2[midx(idx, 0, np)] = iEventTime;
iEventData2[midx(idx, 1, np)] = (double) i;
iEventData2[midx(idx, 2, np)] = (double) npC;
iEventData2[midx(idx, 3, np)] = 1.0;
iEventData2[midx(idx, 4, np)] = LRMessage.LanePointsCenter[i].GetX();
iEventData2[midx(idx, 5, np)] = LRMessage.LanePointsCenter[i].GetY();
iEventData2[midx(idx, 6, np)] = LRMessage.LanePointsCenter[i].GetVariance();
idx++;
}
for (i = 0; i < npR; i++)
{
//set each center lane point in the points packet
iEventData2[midx(idx, 0, np)] = iEventTime;
iEventData2[midx(idx, 1, np)] = (double) i;
iEventData2[midx(idx, 2, np)] = (double) npC;
iEventData2[midx(idx, 3, np)] = 2.0;
iEventData2[midx(idx, 4, np)] = LRMessage.LanePointsRight[i].GetX();
iEventData2[midx(idx, 5, np)] = LRMessage.LanePointsRight[i].GetY();
iEventData2[midx(idx, 6, np)] = LRMessage.LanePointsRight[i].GetVariance();
idx++;
}
//set the event data
Event* NewEvent = new Event();
NewEvent->SetEventType(LOCALMAPROAD_EVENT, iEventTime);
NewEvent->SetEventData(iNumDataRows, iNumDataCols, iEventData);
NewEvent->SetEventData2(iNumDataRows2, iNumDataCols2, iEventData2);
//and push to the event queue
#ifdef SE_PRINTLOGS
//create a separate logging event with the same data
Event* LogEvent = new Event(NewEvent);
#endif
bool spush = ThePosteriorPoseEventQueue->PushEvent(NewEvent);
#ifdef SE_PRINTLOGS
//only log the packet if it successfully entered the event queue
extern EventQueue* TheLoggingQueue;
if (spush == true)
{
TheLoggingQueue->PushEvent(LogEvent);
}
else
{
delete LogEvent;
}
#endif
return;
}
void LocalMapTargetsCallback(LocalMapTargetsMsg LMMessage, LocalMapInterfaceReceiver* LMRX, void* Arg)
{
/*
Callback for handling the targets tracked by LocalMap
INPUTS:
LMMessage - the LocalMap message data containing the targets
LMRX - the local map interface receiver
Arg - a void pointer, not used here
OUTPUTS:
none. Creates an event and places it on the event queue
*/
//gain access to the event queue
extern SynchronizedEventQueue* TheTrackGeneratorEventQueue;
int nt = (int) LMMessage.targets.size();
double iEventTime = LMMessage.timestamp;
//check the event time for valid timestamp
extern CarTime* TheCarTime;
double cct = TheCarTime->CurrentCarTime();
if (fabs(iEventTime - cct) > CT_TIMEJUMPREJECT)
{
//do not allow events with inconsistent timestamps into the event queues
printf("Warning: got localmap targets timestamp %.12lg; expected something near %.12lg\n", iEventTime, cct);
return;
}
int i;
int j;
int k;
int idx;
//declare memory for the header packet and the covariance packet
int iEventType = LOCALMAPTARGETS_EVENT;
int iNumDataRows = nt;
int iNumDataCols = SE_LMTHEADERPACKETSIZE;
double* iEventData = NULL;
if (nt > 0)
{
iEventData = new double[nt*SE_LMTHEADERPACKETSIZE];
}
int iNumDataRows2 = nt;
int iNumDataCols2 = SE_LMTCOVARIANCEPACKETSIZE;
double* iEventData2 = NULL;
if (nt > 0)
{
iEventData2 = new double[nt*SE_LMTCOVARIANCEPACKETSIZE];
}
int np = 0;
for (i = 0; i < nt; i++)
{
//set the packet headers and count the total number of target points
LocalMapTarget CurrentTarget = LMMessage.targets[i];
np += (int) CurrentTarget.points.size();
//HEADER PACKET
//event time
iEventData[midx(i, 0, nt)] = iEventTime;
//target index
iEventData[midx(i, 1, nt)] = (double) i;
//number of targets
iEventData[midx(i, 2, nt)] = (double) nt;
//target type
switch (CurrentTarget.type)
{
case LM_TT_INVALID:
iEventData[midx(i, 3, nt)] = -1.0;
break;
case LM_TT_IBEO:
iEventData[midx(i, 3, nt)] = 101.0;
break;
case LM_TT_IBEOMATURE:
iEventData[midx(i, 3, nt)] = 102.0;
break;
case LM_TT_MOBILEYE:
iEventData[midx(i, 3, nt)] = 103.0;
break;
case LM_TT_RADAR:
iEventData[midx(i, 3, nt)] = 104.0;
break;
case LM_TT_QUASIMATURE:
iEventData[midx(i, 3, nt)] = 105.0;
break;
case LM_TT_MATURE:
iEventData[midx(i, 3, nt)] = 106.0;
break;
default:
iEventData[midx(i, 3, nt)] = -1.0;
}
//target X, Y, Orientation, Speed, Heading, Width
iEventData[midx(i, 4, nt)] = CurrentTarget.x;
iEventData[midx(i, 5, nt)] = CurrentTarget.y;
iEventData[midx(i, 6, nt)] = CurrentTarget.orientation;
iEventData[midx(i, 7, nt)] = CurrentTarget.speed;
iEventData[midx(i, 8, nt)] = CurrentTarget.heading;
iEventData[midx(i, 9, nt)] = CurrentTarget.width;
//number of points
iEventData[midx(i, 10, nt)] = (double) CurrentTarget.points.size();
//COVARIANCE PACKET
//event time
iEventData2[midx(i, 0, nt)] = iEventTime;
//target index
iEventData2[midx(i, 1, nt)] = (double) i;
//number of targets
iEventData2[midx(i, 2, nt)] = (double) nt;
//lower triangle of covariance matrix, in column major order
idx = 3;
for (j = 0; j < 6; j++)
{
for (k = 0; k <= j; k++)
{
iEventData2[midx(i, idx, nt)] = CurrentTarget.covariance[midx(j, k, 6)];
idx++;
}
}
}
//declare memory for target points data
int iNumDataRows3 = np;
int iNumDataCols3 = SE_LMTPOINTSPACKETSIZE;
double* iEventData3 = NULL;
if (np > 0)
{
iEventData3 = new double[np*SE_LMTPOINTSPACKETSIZE];
}
idx = 0;
int npt;
for (i = 0; i < nt; i++)
{
//set the points packet for each target
LocalMapTarget CurrentTarget = LMMessage.targets[i];
npt = (int) CurrentTarget.points.size();
for (j = 0; j < npt; j++)
{
//event time
iEventData3[midx(idx, 0, np)] = iEventTime;
iEventData3[midx(idx, 1, np)] = (double) i;
iEventData3[midx(idx, 2, np)] = (double) nt;
iEventData3[midx(idx, 3, np)] = (double) j;
iEventData3[midx(idx, 4, np)] = (double) npt;
LocalMapPoint CurrentPoint = CurrentTarget.points[j];
iEventData3[midx(idx, 5, np)] = CurrentPoint.GetX();
iEventData3[midx(idx, 6, np)] = CurrentPoint.GetY();
idx++;
}
}
//create the event and push it to the track generator
Event* NewEvent = new Event();
NewEvent->SetEventType(iEventType, iEventTime);
NewEvent->SetEventData(iNumDataRows, iNumDataCols, iEventData);
NewEvent->SetEventData2(iNumDataRows2, iNumDataCols2, iEventData2);
NewEvent->SetEventData3(iNumDataRows3, iNumDataCols3, iEventData3);
#ifdef SE_PRINTLOGS
//create a separate logging event with the same data
Event* LogEvent = new Event(NewEvent);
#endif
bool spush = TheTrackGeneratorEventQueue->PushEvent(NewEvent);
#ifdef SE_PRINTLOGS
//only log the packet if it successfully entered the event queue
extern EventQueue* TheLoggingQueue;
if (spush == true)
{
TheLoggingQueue->PushEvent(LogEvent);
}
else
{
delete LogEvent;
}
#endif
return;
}
void LocalMapLooseClustersCallback(LocalMapLooseClustersMsg LMMessage, LocalMapInterfaceReceiver* LMRX, void* Arg)
{
/*
Callback for the localmap loose clusters message
INPUTS:
LMMessage - the LocalMap message packet
LMRX - pointer to the localmap receiver
Arg - a void pointer (not used)
OUTPUTS:
none. Drops an event onto the event queue.
*/
//gain access to the event queue
extern SynchronizedEventQueue* TheTrackGeneratorEventQueue;
double iEventTime = LMMessage.timestamp;
//check the event time for valid timestamp
extern CarTime* TheCarTime;
double cct = TheCarTime->CurrentCarTime();
if (fabs(iEventTime - cct) > CT_TIMEJUMPREJECT)
{
//do not allow events with inconsistent timestamps into the event queues
printf("Warning: got localmap loose clusters timestamp %.12lg; expected something near %.12lg\n", iEventTime, cct);
return;
}
int nc = (int) LMMessage.clusters.size();
int i;
int j;
int np = 0;
for (i = 0; i < nc; i++)
{
//count the total number of obstacle points across all clusters
LocalMapLooseCluster cc = LMMessage.clusters[i];
np += (int) cc.points.size();
}
//declare memory for the packet and assign values
int iEventType = LOCALMAPPOINTS_EVENT;
int iNumDataRows = np;
int iNumDataCols = SE_LOCALPOINTSPACKETSIZE;
double* iEventData = NULL;
if (np > 0)
{
iEventData = new double[np*SE_LOCALPOINTSPACKETSIZE];
}
int idx = 0;
for (i = 0; i < nc; i++)
{
//pull all clusters and copy into an event packet
LocalMapLooseCluster cc = LMMessage.clusters[i];
int npc = (int) cc.points.size();
for (j = 0; j < npc; j++)
{
//pull each point from the cluster
LocalMapPoint cp = cc.points[j];
//set the event data
iEventData[midx(idx, 0, np)] = iEventTime;
iEventData[midx(idx, 1, np)] = (double) idx;
iEventData[midx(idx, 2, np)] = (double) np;
iEventData[midx(idx, 3, np)] = (double) i;
switch (cc.clusterClass)
{
case LOCALMAP_HighObstacle:
iEventData[midx(idx, 4, np)] = 1.0;
break;
case LOCALMAP_LowObstacle:
iEventData[midx(idx, 4, np)] = 0.0;
break;
}
iEventData[midx(idx, 5, np)] = cp.GetX();
iEventData[midx(idx, 6, np)] = cp.GetY();
//done setting this point
idx++;
}
}
//create the event and push it to the track generator
Event* NewEvent = new Event();
NewEvent->SetEventType(iEventType, iEventTime);
NewEvent->SetEventData(iNumDataRows, iNumDataCols, iEventData);
#ifdef SE_PRINTLOGS
//create a separate logging event with the same data
Event* LogEvent = new Event(NewEvent);
#endif
bool spush = TheTrackGeneratorEventQueue->PushEvent(NewEvent);
#ifdef SE_PRINTLOGS
//only log the packet if it successfully entered the event queue
extern EventQueue* TheLoggingQueue;
if (spush == true)
{
TheLoggingQueue->PushEvent(LogEvent);
}
else
{
delete LogEvent;
}
#endif
return;
}
void JasonCallback(RoadFitterOutput JasonMessage, RoadFitterInterfaceReceiver* JRX, RoadFitterID ID, void* Arg)
{
/*
Called when jason's algorithm returns a road fix
INPUTS:
JasonMessage - the results stored in Jason's data packet
JRX - pointer to the interface receiver (not used here)
ID - enumerated ID indicating which camera was used to create the message
Arg - null pointer (not used here)
OUTPUTS:
none. Reformats the packet into an event and drops it onto ThePosteriorPoseEventQueue
*/
int i;
int j;
//extract the number of jason's road segments (always RF_MAX_FITS)
int nj = RF_VALID_FITS;
//gain access to the event queue
extern SynchronizedEventQueue* ThePosteriorPoseEventQueue;
//set timestamp from car time
double iEventTime = JasonMessage.carTime;
//check the event time for valid timestamp
extern CarTime* TheCarTime;
double cct = TheCarTime->CurrentCarTime();
if (fabs(iEventTime - cct) > CT_TIMEJUMPREJECT)
{
//do not allow events with inconsistent timestamps into the event queues
printf("Warning: got roadfinder timestamp %.12lg; expected something near %.12lg\n", iEventTime, cct);
return;
}
//declare space for the event data
//NOTE: this memory is cleared automatically when the event is pulled from the queue
Event* NewEvent = new Event();
int iEventType;
double* iEventData = new double[nj*SE_JASONROADPACKETSIZE];
for (i = 0; i < nj; i++)
{
for (j = 0; j < SE_JASONROADPACKETSIZE; j++)
{
iEventData[midx(i, j, nj)] = 0.0;
}
}
for (i = 0; i < nj; i++)
{
//timestamp (synchronized vehicle time)
iEventData[midx(i, 0, nj)] = iEventTime;
//index of the segmentation and number of segmentations
iEventData[midx(i, 1, nj)] = (double) (i);
iEventData[midx(i, 2, nj)] = (double) (nj);
//number of lines / boundaries found (0 - 4)
int nb = JasonMessage.roadFits[i].borders_observed;
iEventData[midx(i, 3, nj)] = (double) nb;
for (j = 0; j < nb; j++)
{
//distance to boundaries found (m from camera, positive for lines to the left of center)
iEventData[midx(i, j+4, nj)] = JasonMessage.roadFits[i].border_offsets[j];
//boundary types
switch (JasonMessage.roadFits[i].observed_border_types[j])
{
case RoadFitterOutputData::BT_SingleLaneLine:
iEventData[midx(i, j+8, nj)] = 1.0;
break;
case RoadFitterOutputData::BT_Edge:
iEventData[midx(i, j+8, nj)] = 0.0;
break;
default:
iEventData[midx(i, j+8, nj)] = 0.0;
break;
}
}
//heading of road wrt camera (m/m, positive for roads heading right)
iEventData[midx(i, 12, nj)] = -JasonMessage.roadFits[i].road_heading;
//curvature of road wrt camera (m/m^2, positive for roads curving right)
iEventData[midx(i, 13, nj)] = -JasonMessage.roadFits[i].system_curvature;
//confidence in segmentation fit
iEventData[midx(i, 14, nj)] = JasonMessage.roadFits[i].confidence;
}
switch (ID)
{
case RF_CTR:
iEventType = FRONTJASONROAD_EVENT;
break;
default:
iEventType = INVALID_EVENT;
break;
}
//store the event data in the event container
NewEvent->SetEventType(iEventType, iEventTime);
NewEvent->SetEventData(nj, SE_JASONROADPACKETSIZE, iEventData);
#ifdef SE_PRINTLOGS
//create a separate logging event with the same data
Event* LogEvent = new Event(NewEvent);
#endif
//drop the event onto the event queue
bool spush = ThePosteriorPoseEventQueue->PushEvent(NewEvent);
#ifdef SE_PRINTLOGS
//only log the packet if it successfully entered the event queue
extern EventQueue* TheLoggingQueue;
if (spush == true)
{
TheLoggingQueue->PushEvent(LogEvent);
}
else
{
delete LogEvent;
}
#endif
return;
}
void StoplineCallback(StopLineMessage SLMessage, StopLineInterfaceReceiver* SRX, void* Arg)
{
/*
Sensor callback for receiving stopline information from Aaron's stopline algorithm
INPUTS:
SLMessage - class containing the stopline packet data
SRX - stopline interface receiver (not used here)
Arg - void pointer (not used here)
OUTPUTS:
none. Creates an event for the stopline information and pushes the
event onto ThePosteriorPoseEventQueue
*/
int i;
//gain access to the event queue
extern SynchronizedEventQueue* ThePosteriorPoseEventQueue;
//set timestamp from car time
double iEventTime = SLMessage.carTime;
//check the event time for valid timestamp
extern CarTime* TheCarTime;
double cct = TheCarTime->CurrentCarTime();
if (fabs(iEventTime - cct) > CT_TIMEJUMPREJECT)
{
//do not allow events with inconsistent timestamps into the event queues
printf("Warning: got stopline timestamp %.12lg; expected something near %.12lg\n", iEventTime, cct);
return;
}
//extract the number of stoplines
int ns = SLMessage.numStopLines;
//declare space for the event data
//NOTE: this memory is cleared automatically when the event is pulled from the queue
Event* NewEvent = new Event();
double* iEventData = NULL;
if (ns > 0)
{
iEventData = new double[ns*SE_STOPLINEPACKETSIZE];
}
for (i = 0; i < ns; i++)
{
//timestamp (synchronized vehicle time)
iEventData[midx(i, 0, ns)] = iEventTime;
//index of the stopline and number of stoplines
iEventData[midx(i, 1, ns)] = (double) (i);
iEventData[midx(i, 2, ns)] = (double) (ns);
//distance to stopline
iEventData[midx(i, 3, ns)] = SLMessage.stopLines[i].distance;
//confidence in stopline
iEventData[midx(i, 4, ns)] = SLMessage.stopLines[i].confidence;
}
//store the event data in the event container
NewEvent->SetEventType(STOPLINE_EVENT, iEventTime);
NewEvent->SetEventData(ns, SE_STOPLINEPACKETSIZE, iEventData);
#ifdef SE_PRINTLOGS
//create a separate logging event with the same data
Event* LogEvent = new Event(NewEvent);
#endif
//drop the event onto the event queue
bool spush = ThePosteriorPoseEventQueue->PushEvent(NewEvent);
#ifdef SE_PRINTLOGS
//only log the packet if it successfully entered the event queue
extern EventQueue* TheLoggingQueue;
if (spush == true)
{
TheLoggingQueue->PushEvent(LogEvent);
}
else
{
delete LogEvent;
}
#endif
return;
}