main()
{
int fd,c, res;
struct termios oldtio,newtio;
char buf[255];
char buf2[255];
// ssmのイニシャライズ
initSSM();
DORDER.create(5.0, 1.0);
fd = open(MODEMDEVICE, O_WRONLY | O_NOCTTY );
if (fd <0) {perror(MODEMDEVICE); exit(-1); }
tcgetattr(fd,&oldtio); /* 現在のポート設定を待避 */
bzero(&newtio, sizeof(newtio));
newtio.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
/* set input mode (non-canonical, no echo,...) */
newtio.c_lflag = 0;
newtio.c_cc[VTIME] = 0; /* キャラクタ間タイマは未使用 */
newtio.c_cc[VMIN] = 5; /* 5文字受け取るまでブロックする */
tcflush(fd, TCIFLUSH);
tcsetattr(fd,TCSANOW,&newtio);
while(1)
{
DORDER.readNew();
cout << DORDER.data.order << endl;
buf[0] = 'o';
buf2[0] = 'c';
if(DORDER.data.order >= 1)
{
write(fd, buf, 1);
}else{
write(fd, buf2, 1);
}
usleep(1000);
}
tcsetattr(fd,TCSANOW,&oldtio);//設定をもとに戻す
close(fd);
o}
int main (int argc, char *argv[])
{
//-SSM-//
initSSM();
OBJECT.create(5.0,1.0);
SCAN_DATA.open(SSM_READ);
AREA.create(5.0, 1.0);
//-クラスと構造体-//
Step_buf raw_data_U; //生データ
Step_buf raw_data_D; //生データ
Step_buf ped_data; //加工後のデータ
LS3D background_U; //環境データ
LS3D background_D; //環境データ
//-時間の制御用-//
double time_MovingObjects_are_detected = get_time(); // 動体が検出された時の時刻
double time_MovingObjects_gets_out = get_time(); // 動体がいなくなったときの時刻
double time_get_background = 0.0;
//-初期化-//
initialize(raw_data_U, raw_data_D, background_U, background_D);
time_get_background = get_time();
// raw_data.out_csv(); // デバッグ用
//-ループ開始-//
while(1)
{
if (SCAN_DATA.readNew())
{
double loop_start_time = get_time();
if (SCAN_DATA.data.det == 'U')
{
//-読み込みデータのセット-//
raw_data_U.set_data(SCAN_DATA.data.det, SCAN_DATA.data.dist, SCAN_DATA.data.x, SCAN_DATA.data.y, SCAN_DATA.data.z);
//-差分データの計算-//
cal_background_diff (raw_data_U, ped_data, background_U);
//-エリアフィルタの適用-//
apply_area_filter(ped_data);
//-差分データをSSM構造体にセット-//
OBJECT.data.det = ped_data.det[CUR_INDEX];
for (int i=0; i<STEP_NUM; i++)
{
OBJECT.data.dist[i] = ped_data.dist[i][CUR_INDEX];
OBJECT.data.x[i] = ped_data.x[i][CUR_INDEX];
OBJECT.data.y[i] = ped_data.y[i][CUR_INDEX];
OBJECT.data.z[i] = ped_data.z[i][CUR_INDEX];
}
}
if (SCAN_DATA.data.det == 'D')
{
//-読み込みデータのセット-//
raw_data_D.set_data(SCAN_DATA.data.det, SCAN_DATA.data.dist, SCAN_DATA.data.x, SCAN_DATA.data.y, SCAN_DATA.data.z);
//-差分データの計算-//
cal_background_diff (raw_data_D, ped_data, background_D);
//-エリアフィルタの適用-//
apply_area_filter(ped_data);
//-差分データをSSM構造体にセット-//
OBJECT.data.det = ped_data.det[CUR_INDEX];
for (int i=0; i<STEP_NUM; i++)
{
OBJECT.data.dist[i] = ped_data.dist[i][CUR_INDEX];
OBJECT.data.x[i] = ped_data.x[i][CUR_INDEX];
OBJECT.data.y[i] = ped_data.y[i][CUR_INDEX];
OBJECT.data.z[i] = ped_data.z[i][CUR_INDEX];
}
}
//-静止物体が存在するかの判定-//
// AREA.data.hasObjects = judge_Objects_exist(ped_data);
//-動体が存在するかの判定-//
AREA.data.hasMovingObjects = judge_MovingObjects_exist(ped_data);
if ( AREA.data.hasMovingObjects == true)
{
// 動物体を検出したときの時刻を取得
time_MovingObjects_are_detected = get_time();
}else{
// 動物体が存在しないときの時刻を取得
time_MovingObjects_gets_out = get_time();
// 最後に物体を検出してからの時間を算出
double timelength_noMovingObjects = time_MovingObjects_gets_out - time_MovingObjects_are_detected;
cout << "timelength_noMovingObjects = " << timelength_noMovingObjects << endl;
// 動物体が長い間存在していなかったら、環境データを更新する。
if (timelength_noMovingObjects > 3
&& (get_time() - time_get_background) > 180)
{
get_background (raw_data_U, background_U);
get_background (raw_data_D, background_D);
time_get_background = get_time();
time_MovingObjects_are_detected = get_time();
time_MovingObjects_gets_out = get_time();
}
}
//-SSMの書き込み-//
OBJECT.write();
AREA.write();
double loop_end_time = get_time();
sleep_const_freq(loop_start_time, loop_end_time, FREQ);
}else{
usleep(10000); // CPU使用率100%対策
}
}
//-SSMクローズ-//
OBJECT.release();
SCAN_DATA.close();
AREA.release();
endSSM();
return 0;
}
int main(int argc, char ** argv)
{
uint64_t leftGUID,rightGUID;
//設定ファイル読み込み用クラス by KAW
TxtConfigurator tc(CONFIG_FILE);
//SSM初期化
if(!initSSM()){
cout << "SSM Initialize error." << endl;
return -1;
}
//設定ファイルの読み込み。
//設定ファイルの読み込み。
#ifdef IS32BIT
cout << "This is 32 bit environment" << endl;
tc.set("LEFT_GUID",(long long *)&leftGUID);
tc.set("RIGHT_GUID",(long long *)&rightGUID);
#else
cout << "This is 64 bit environment" << endl;
tc.set("LEFT_GUID",(long *)&leftGUID);
tc.set("RIGHT_GUID",(long *)&rightGUID);
#endif
tc.set("FRAME_RATE",&frame_rate);
tc.set("COLOR_MOED",&color_mode);
tc.set("SHOW_IMAGE",&isShowImage);
tc.set("SHOW_FRAME_RATE",&isShowFrameRate);
tc.set("SHOW_WHITE_CALIBRATION",&isShowWhiteCalibration);
tc.set("SHOW_CONTROL",&isShowControl);
tc.set("WHAT_IS_GRAY",&WhatIsGray);
tc.set("SHUTTER_SPEED",&ShutterSpeed);
tc.substitute();//必須
tc.close(); //してもしなくてもいい。
cout << "-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=" << endl;
switch(color_mode){
case 0:
cout << "Color Mode = OFF" << endl;
color_mode = DC1394_VIDEO_MODE_640x480_MONO8;
isColor = false;
break;
case 1:
cout << "Color Mode = ON" << endl;
color_mode = DC1394_VIDEO_MODE_640x480_RGB8;
isColor = true;
break;
default:
cout << "Color Mode Not Set : 0(Mono) 1(Color) Only" << endl;
cout << "Color Mode = ON" << endl;
color_mode = DC1394_VIDEO_MODE_640x480_RGB8;
isColor = true;
break;
}
switch(frame_rate){
case 7:
frame_rate = DC1394_FRAMERATE_7_5;
cout << "Frame Rate = 7.5" << endl;
break;
case 15:
frame_rate = DC1394_FRAMERATE_15;
cout << "Frame Rate = 15" << endl;
break;
case 30:
cout << "Frame Rate = 30" << endl;
frame_rate = DC1394_FRAMERATE_30;
break;
case 60:
frame_rate = DC1394_FRAMERATE_60;
cout << "Frame Rate = 60" << endl;
break;
default:
cout << "Frame Rate Not Set : 7(7.5), 15, 30, 60 Only" << endl;
cout << "Frame Rate = 15" << endl;
frame_rate = DC1394_FRAMERATE_15;
break;
}
if( !stereo_images.create( 1.5, 1 / 15.0) ){
return 1;
}
setSigInt();
bool isEnd = false;
while(!isEnd){
cout << "Start Main Loop " << endl;
isEnd = MainLoop(leftGUID,rightGUID);
}
stereo_images.release();
endSSM();
cout << "End Successfully." << endl;
return 0;
}
int main (int argc, char *argv[])
{
//-SSM-//
initSSM();
OBJECT.create(5.0,1.0);
SCAN_DATA.open(SSM_READ);
get_background(&background_U, &background_D);
// ofstream ofs;
// ofs.open("background");
// for (int i = 0; i < beam_num; i++)
// {
// ofs << background_U.dist[i] << endl;
// }
// for (int i = 0; i < beam_num; i++)
// {
// ofs << background_D.dist[i] << endl;
// }
// ofs.close();
cout << "get background" << endl;
while(1) //データ取得ループ
{
if(SCAN_DATA.readNew()) //readNew
{
time_1 = get_time();
if(SCAN_DATA.data.det == 'U')
{
OBJECT.data.det = 'U';
for(int i=0; i<beam_num; i++)
{
dist_diff[i] = fabs(SCAN_DATA.data.dist[i] - background_U.dist[i]);
if(dist_diff[i] <= diff_min)
{
OBJECT.data.dist[i] = 0.0;
OBJECT.data.x[i] = 0.0;
OBJECT.data.y[i] = 0.0;
OBJECT.data.z[i] = 0.0;
}else{//(dist_diff[i] > diff_min /*&& dist_diff[i] < diff_max*/){
OBJECT.data.dist[i] = SCAN_DATA.data.dist[i];
OBJECT.data.x[i] = SCAN_DATA.data.x[i];
OBJECT.data.y[i] = SCAN_DATA.data.y[i];
OBJECT.data.z[i] = SCAN_DATA.data.z[i];
}
}
}
if(SCAN_DATA.data.det == 'D')
{
OBJECT.data.det = 'D';
for(int i=0; i<beam_num; i++)
{
dist_diff[i] = fabs(SCAN_DATA.data.dist[i] - background_D.dist[i]);
if(dist_diff[i] <= diff_min )
{
OBJECT.data.dist[i] = 0.0;
OBJECT.data.x[i] = 0.0;
OBJECT.data.y[i] = 0.0;
OBJECT.data.z[i] = 0.0;
}else{//(dist_diff[i] > diff_min /*&& dist_diff[i] < diff_max*/){
OBJECT.data.dist[i] = SCAN_DATA.data.dist[i];
OBJECT.data.x[i] = SCAN_DATA.data.x[i];
OBJECT.data.y[i] = SCAN_DATA.data.y[i];
OBJECT.data.z[i] = SCAN_DATA.data.z[i];
}
}
}
OBJECT.write();
time_2 = get_time();
usleep(freq*1000000 - (time_2 - time_1)*1000000);
time_3 = get_time();
cout << "time = " << time_3 - time_1 << endl;
}else{ //readNew
usleep(1000); //CPU使用率100％対策
}
} //データ取得ループ
//-SSMクローズ-//
OBJECT.release();
SCAN_DATA.close();
endSSM();
return 0;
}