BOOL OpenOut( LPTSTR lpf, HANDLE * pHand, BOOL bAppend )
{
BOOL bRet = FALSE;
HANDLE h;
if( bAppend )
{
h = grmOpenFile( lpf, pHand, 1 );
if( VFH(h) )
{
LONG lg = 0;
SetFilePointer(h, 0, &lg, FILE_END );
bRet = TRUE;
}
}
else
{
h = grmCreateFile( lpf );
if( VFH(h) )
{
*pHand = h;
bRet = TRUE;
}
}
return bRet;
}
///////////////////////////////////////////////////////////////////////////////
// FUNCTION : doutput
// Return type: VOID
// Argument : LPTSTR lps
// Description: DIRECT output to the various channels
// WITHOUT end-of-line checking.
///////////////////////////////////////////////////////////////////////////////
VOID doutput( LPTSTR lps )
{
DWORD dwl = strlen(lps);
outstg( gw_hStdOut, lps, dwl );
#ifndef NDEBUG
if( VFH(hDbgFile) )
outstg( hDbgFile, lps, dwl );
#endif // !NDEBUG
if( VFH( g_hOutFile ) )
outstg( g_hOutFile, lps, dwl );
}
void CloseDiagFile( HANDLE * lpHF )
{
HANDLE h = *lpHF;
if( VFH(h) )
CloseHandle(h);
*lpHF = 0;
}
void oi( LPTSTR lps )
{
DWORD dwl = strlen(lps);
if( dwl && VFH(gw_hStdOut) )
outstg( gw_hStdOut, lps, dwl );
#ifndef NDEBUG
WriteDiagFile(lps);
#endif // !NDEBUG
if( dwl && VFH( g_hOutFile ) )
{
if( !outstg(g_hOutFile, lps, dwl) )
{
CloseHandle(g_hOutFile);
g_hOutFile = INVALID_HANDLE_VALUE;
}
}
}
void OpenRadar::laser_proc()
{
int start_step = 0;
int end_step = num_data - 2;
float dis;
// first apply a 3*3 box filter to ranges data
//for (int i = 1; i < MAX_frame - 1; i++)
// for (int j = 1; j < end_step - start_step; j++)
// ranges[i][j] = (ranges[i+1][j]+ranges[i][j]+ranges[i-1][j])*0.33333333333;
//for (int i = 1; i < MAX_frame - 1; i++)
// for (int j = 1; j < end_step - start_step; j++)
// ranges[i][j] = (ranges[i][j+1]+ranges[i][j]+ranges[i][j-1])*0.33333333333;
//Inilize RadarTheta
for(int i = 0; i < end_step ;i++)
{
RadarTheta[i] = i*angle_increment + angle_min;
//from noisy laser measurement to a relatively stable mesurement
dis = denoise_across_frame(i);
//cout << i<<"," << dis <<"|";
/*
if (dis - 300 > range_min)
//RadarRho[i] = dis - 300; //add a safe distance 30cm
RadarRho[i] = dis;
else
RadarRho[i] = dis;
//if (RadarRho[i] > 2500) RadarRho[i] = -1;
*/
if(dis > range_max) RadarRho[i] = range_max;
if(dis < range_min) RadarRho[i] = range_min;
if(dis >= range_min && dis<= range_max) RadarRho[i] = dis;
}
RadarDataCnt = num_data;
VFH();
//ROS_INFO("calculated scale: %f", v_scale);
//ROS_INFO("calculated angle: %f", v_angle/M_PI*180);
RadarBreak();
DrawRadarData();
cvShowImage("Radar",RadarImage);
if (cvWaitKey(3) == 27)
ros::shutdown();
}
VOID prt_init( VOID )
{
if( !gw_fUsePrintf && !gw_fDnUseP )
{
#ifdef WIN32
gw_hStdOut = GetStdHandle( STD_OUTPUT_HANDLE );
#else
gw_hStdOut = 1;
#endif
if( !VFH(gw_hStdOut) )
{
gw_hStdOut = 0;
gw_fUsePrintf = TRUE;
printf( "\nERROR: Get STD_OUTPUT_HANDLE FAILED!!!"MEOR );
}
gw_fDnUseP = TRUE;
}
}
void WriteDiagFile( LPTSTR lpd )
{
int i = strlen(lpd);
if( i && ( hDbgFile == 0 ) )
{
LPTSTR pfn = szDbgFile;
GetModulePath(pfn);
strcat(pfn, szDefDbg);
OpenDiagFile( pfn, &hDbgFile, FALSE ); // TRUE );
}
if( i && VFH(hDbgFile) )
{
if( !outstg( hDbgFile, lpd, i ) )
{
CloseHandle( hDbgFile );
hDbgFile = INVALID_HANDLE_VALUE;
}
}
}
void WriteDiagFile_OK_but_why( LPTSTR lpd )
{
int i, j, k;
char buf[260];
char c, d;
LPTSTR lpb;
i = 0;
if( lpd )
i = lstrlen( lpd );
if(i)
{
if( hDbgFile == 0 )
{
GetModuleFileName(NULL, szDbgFile, 256);
lpb = strrchr( szDbgFile, '\\' );
if(lpb)
lpb++;
else
lpb = szDbgFile;
strcpy(lpb, szDefDbg);
OpenDiagFile( &szDbgFile[0], &hDbgFile, FALSE ); // TRUE );
}
if( VFH(hDbgFile) )
{
lpb = &buf[0];
k = 0;
d = c = 0;
for( j = 0; j < i; j++ )
{
c = lpd[j];
if( c == 0x0a ) // A LF
{
if( d != 0x0d )
{
lpb[k++] = 0x0d;
}
}
else if( c == 0x0d ) // A CR
{
if( (j+1) < i ) // If more
{
if( lpd[j+1] != 0x0a )
{
lpb[k++] = c;
c = 0x0a;
}
}
else
{
lpb[k++] = c;
c = 0x0a;
}
}
lpb[k++] = c;
d = c; // Keep previous put
}
if( k )
{
if( c != 0x0a )
{
lpb[k++] = 0x0d;
lpb[k++] = 0x0a;
}
if( !outstg( hDbgFile, lpb, k ) )
{
CloseHandle( hDbgFile );
hDbgFile = INVALID_HANDLE_VALUE;
}
}
}
}
}
void prt_OK_maybe( LPTSTR lps )
{
BOOL flg;
DWORD dwLen = strlen(lps);
DWORD dwi, dwk;
static TCHAR _s_buf[MXIOB+264];
char c, d;
LPTSTR lpout;
if( gw_fUsePrintf )
{
//printf( lps );
printf( lps, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 );
}
else
{
if( dwLen )
{
lpout = &_s_buf[0];
dwk = 0;
d = 0;
for( dwi = 0; dwi < dwLen; dwi++ )
{
c = lps[dwi];
if( c == 0x0d )
{
if( (dwi+1) < dwLen )
{
if( lps[dwi+1] != 0x0a )
{
lpout[dwk++] = c;
c = 0x0a;
}
}
else
{
lpout[dwk++] = c;
c = 0x0a;
}
}
else if( c == 0x0a )
{
if( d != 0x0d )
{
lpout[dwk++] = 0x0d;
}
}
lpout[dwk++] = c;
d = c;
if( dwk >= MXIOB )
{
flg = outstg( gw_hStdOut, lpout, dwk );
dwk = 0;
if( !flg )
break;
}
}
if( dwk )
{
outstg( gw_hStdOut, lpout, dwk );
}
}
}
#ifndef NDEBUG
WriteDiagFile(lps);
#endif // !NDEBUG
if( dwLen && VFH( g_hOutFile ) )
{
if( outstg(g_hOutFile, lps, dwLen) )
{
if( lps[dwLen-1] >= ' ' )
outstg( g_hOutFile, MEOR, 2 );
}
else
{
CloseHandle(g_hOutFile);
g_hOutFile = INVALID_HANDLE_VALUE;
}
}
}
