VisionSTCam(ros::NodeHandle _nh):nh(_nh),it(_nh)
{
startingOffset = 3*(FRAME_WIDTH*(FRAME_HEIGHT-270));
checkSize = 3*640;
endingOffset = startingOffset + checkSize;
avgIntensity, lastAvgIntensity = 0;
lastAccumVal = 0;
curExposure, calcExposure, curGlobalGain;
gainStep = 1;
intensityDiff = 0, lastIntensityDiff = 1;
dividingFactor = DEFAULT_DIVIDING_FACTOR;
left_pub = it.advertise("stereo/left/image_raw", 1);
right_pub = it.advertise("stereo/right/image_raw", 1);
disparity_pub = it.advertise("stereo/disparity/image_raw",1);
//Control Panel
cv::namedWindow("Control Intensity", CV_WINDOW_AUTOSIZE);
cv::resizeWindow("Control Intensity", 480, 400);
cv::createTrackbar("Global Gain", "Control Intensity", &m_cScrollBarGlobalGain, 200);
cv::createTrackbar("Exposure", "Control Intensity", &m_cScrollBarExposure, 100);
m_cScrollBarGlobalGain = DEFAULT_GLOBAL_GAIN;
m_cScrollBarExposure = DEFAULT_EXPOSURE;
leftImg = cv::Mat(FRAME_HEIGHT, FRAME_WIDTH, CV_8UC3);
rightImg = cv::Mat(FRAME_HEIGHT, FRAME_WIDTH, CV_8UC3);
disparityImg = cv::Mat(FRAME_HEIGHT, FRAME_WIDTH, CV_8UC1);
m_sockUdpForFindingCameras = boost::shared_ptr<CVcUdp>(new CVcUdp());
if(!GetMyIpAddr()){
ROS_ERROR("check your NIC. found IP is 127.0.0.1");
isInit=false;
}
m_bMyConfigIsReady = m_clMyConfigInfo.ReadConfigInfo(m_szMyIpAddr);
if(!m_bMyConfigIsReady) // CONFIG file 정보가 제대로 읽혀지지 않았으면
{
ROS_WARN("Configuration Info Err.!");
ROS_WARN("Default Value set.!");
}
if(!CreateSocketUDP()){
ROS_ERROR("error occured when creating UDP socket");
isInit = false;
}
isInit=true;
m_clCmdProc = boost::shared_ptr<CCmdPacket>(new CCmdPacket(&m_sockSendCmdUdp, m_sockResponseUdp.get()));
m_clCmdProc->VstCmdGetStatus_IsStarted(m_clMyConfigInfo.m_strCameraIpAddr);
usleep(200000);
if(m_sockResponseUdp->m_nIsCamStatusRun == STATUS_STOP) // 받은 메시지가 Stop 상태이면
{
ROS_INFO("m_sockResponseUdp STATUS:STOP %s",m_clMyConfigInfo.m_strCameraIpAddr);
int rtn = m_clCmdProc->VstCmdInitialize(m_clMyConfigInfo.m_strCameraIpAddr);
if(rtn == CMD_ERROR)
{
ROS_ERROR("Init Command Error!");
}
else
{
ROS_ERROR("Init Command OK!");
}
m_sockResponseUdp->m_nIsCamStatusRun = STATUS_NO_RESPONSE;
m_clCmdProc->VstCmdGetStatus_IsStarted(m_clMyConfigInfo.m_strCameraIpAddr);
usleep(100000); // 기다렸다가...
///OnBnClickedRun();
m_bForcedStop = false;
// TODO: 여기에 컨트롤 알림 처리기 코드를 추가합니다.
rtn = m_clCmdProc->VstCmdStart(m_clMyConfigInfo.m_strCameraIpAddr);
if(rtn == CMD_ERROR)
{
ROS_ERROR("Run Command Error!");
}
else
{
ROS_INFO("Run Command OK!");
}
m_sockResponseUdp->m_nIsCamStatusRun = STATUS_NO_RESPONSE;
m_clCmdProc->VstCmdGetStatus_IsStarted(m_clMyConfigInfo.m_strCameraIpAddr);
// 표시 화면 생성
///SetTimer(TIMER_IMG_DISPLAY, msec_IMAGE_DISPLAY_CYCLE, NULL);
///SetTimer(TIMER_DISPLAY_IMG_DATA_COMM_STATUS, 1000, NULL);
usleep(100000); // 기다렸다가...
SetGlobalGain(m_cScrollBarGlobalGain);
SetExposure(m_cScrollBarExposure);
}
else if(m_sockResponseUdp->m_nIsCamStatusRun == STATUS_RUN) // 현재 메시지를 송출 중이라면
{
ROS_INFO("m_sockResponseUdp STATUS:RUN");
///OnBnClickedRun();
m_bForcedStop = false;
//TODO: 여기에 컨트롤 알림 처리기 코드를 추가합니다.
int rtn = m_clCmdProc->VstCmdStart(m_clMyConfigInfo.m_strCameraIpAddr);
if(rtn == CMD_ERROR)
{
ROS_ERROR("Run Command Error!");
}
else
{
ROS_INFO("Run Command OK!");
}
m_sockResponseUdp->m_nIsCamStatusRun = STATUS_NO_RESPONSE;
m_clCmdProc->VstCmdGetStatus_IsStarted(m_clMyConfigInfo.m_strCameraIpAddr);
// 표시 화면 생성
///SetTimer(TIMER_IMG_DISPLAY, msec_IMAGE_DISPLAY_CYCLE, NULL);
///SetTimer(TIMER_DISPLAY_IMG_DATA_COMM_STATUS, 1000, NULL);
//usleep(100000); // 기다렸다가...
SetGlobalGain(DEFAULT_GLOBAL_GAIN);
SetExposure(DEFAULT_EXPOSURE);
///SetTimer(TIMER_UDP_DATA_COMM_DISPLAY, msec_IMAGE_DISPLAY_CYCLE, NULL);
}
}
int
main(int argc,char *argv[],char *envp[])
{
if(argc != 3 && argc != 4){
fprintf(stderr,"%%./gnutella.exe gport hport or %%./gnutella.exe gport hport env\n");
fprintf(stderr, "gport is the gnutella connection port number.\n");
fprintf(stderr, "hport is the http connection port number.\n");
fprintf(stderr, "env is your environment.\n");
fprintf(stderr, " 0: Mac OS X with wired LAN(Default).\n");
fprintf(stderr, " 1: Mac OS X with wireless LAN.\n");
fprintf(stderr, " 2: Linux.\n");
fprintf(stderr, " 3: Windows.\n");
exit(-1);
}
/******************************************var****************************************/
//net
int gnufd;
int httpfd;
int newfd;
//select
fd_set rdfds;
//tmp_aadr
struct sockaddr_in myskt,peerskt;
struct in_addr tmp;
//socklen_t my_sktlen = sizeof(myskt);
socklen_t peer_sktlen = sizeof(peerskt);
//analyze
char cmd[CMD_SIZE];
Cmd_tbl *pcmd;
char *p;
int myargc;
char *myargv[ARGV_SIZE];
/******************************************var end****************************************/
/*****************************************set addr*********************************/
char *os = MAC_WIRED;
if(argc >= 4){
int env = atoi(argv[3]);
if(env == 0) os = MAC_WIRED;
if(env == 1) os = MAC_WIRELESS;
if(env == 2) os = LINUX;
if(env == 3) os = WIN;
}
myip = tmp.s_addr = GetMyIpAddr(os);
memset(&myskt,0,sizeof(myskt));
myskt.sin_addr.s_addr = Get_Myip();
myskt.sin_family = AF_INET;
//default
myportg = GNU_PORT;
myskt.sin_port = htons(GNU_PORT);//GNU port 6346(Big)
if(argc >= 3){//%a.out port1 port2
myportg = atoi(argv[1]);
myskt.sin_port = htons(myportg);
}
if((gnufd = Gnutella_Create_Waitfd(myskt)) < 0){//Create Gnutella socket
fprintf(stderr,"Cnutella_Create_Waitfd() error!\n");
exit(-1);
}
//default
myporth = HTTP_PORT;
myskt.sin_port = htons(HTTP_PORT);//HTTP port 10010(Big)
if(argc >= 3){//%a.out port1 port2
myporth = atoi(argv[2]);
myskt.sin_port = htons(myporth);
}
if((httpfd = Gnutella_Create_Waitfd(myskt)) < 0){//Create HTTP socket
fprintf(stderr,"Cnutella_Create_Waitfd() error!\n");
exit(-1);
}
//default
share_dir = DEFAULT_DIR;
/*****************************************set addr end*********************************/
//init
Init_Local_Sharelist(Get_Share_Dir());
Init_Net_Sharelist();
Init_Thtbl();
Init_Ping_List();
Init_Query_List();
myspeed = DEFAULT_MYSPEED;
//print
printf("+++IP+++ %s\n",inet_ntoa(tmp));
printf("+++gnuport+++ %d\n",Get_Myportg());
printf("+++httpport+++ %d\n",Get_Myporth());
printf("+++shared dir+++ %s\n",Get_Share_Dir());
printf("+++shared file num+++ %d\n",Get_Local_Share_Num());
// printf("+++responding speed+++ %d\n",Get_Myspeed());
Print_Local_Sharelist();
/*****************************while start*************************/
while(1){
fflush(stdin);
fprintf(stderr,"[gnutella]%%");//prompt
FD_ZERO(&rdfds);//init
FD_SET(STDIN_FD,&rdfds);//keyboard
FD_SET(gnufd,&rdfds);//gnutella
FD_SET(httpfd,&rdfds);//http
if(select(httpfd+1,&rdfds,NULL,NULL,NULL) < 0){//monitor & block
perror("select");
continue;
}
if(FD_ISSET(STDIN_FD,&rdfds)){//from keyboard,input cmd
//analyze
if(fgets(cmd,sizeof(cmd),stdin) == NULL){
if(feof(stdin))
fprintf(stderr,"EOF pushed\n");
else
fprintf(stderr,"fgets() error!\n");
continue;
}
if((p = Search_Char(cmd,'\n',sizeof(cmd))) == NULL){
fprintf(stderr,"Search_Char() error!\n");
continue;
}
*p = '\0';
getargs(cmd,&myargc,myargv);
if(myargc <= 0)
continue;
//analyze end
for(pcmd = cmdtbl; pcmd->cmd != NULL; pcmd++){
if(strcmp(myargv[0],pcmd->cmd) == 0){
if( (*pcmd->pf)(myargc,myargv) < 0){//cmd handler
fprintf(stderr,"cmd %s error\n",pcmd->cmd);
}
break;
}
}
if(pcmd->cmd == NULL)
fprintf(stderr,"Unknown Command\n");
}
if(FD_ISSET(gnufd,&rdfds)){//from gnutella socket
memset(&peerskt,0,sizeof(peerskt));
peer_sktlen = sizeof(peerskt);
if((newfd = accept(gnufd,(struct sockaddr *)&peerskt,&peer_sktlen)) < 0){
perror("accept");
continue;
}
if(Gnutella_Accept(newfd) < 0){//bad msg or timeout
fprintf(stderr,"gnutella accept fail!!\n");
close(newfd);
continue;
}
printf("gnutella accept OK!!\n");
/* create thread */
if(Create_Thread(newfd,TYPE_GNU) < 0){//thread create error
fprintf(stderr,"Create_Gnu_Thread() error!\n");
close(newfd);
continue;
}
printf("create thread success!!\n");
}
if(FD_ISSET(httpfd,&rdfds)){//from http socket
printf("coming http port!\n");
memset(&peerskt,0,sizeof(peerskt));
peer_sktlen = sizeof(peerskt);
if((newfd = accept(httpfd,(struct sockaddr *)&peerskt,&peer_sktlen)) < 0){
perror("accept");
continue;
}
/* create thread */
if(Create_Thread(newfd,TYPE_HTTP) < 0){//thread create error
fprintf(stderr,"Create_Http_Thread() error!\n");
close(newfd);
continue;
}
printf("create thread success!!\n");
}
}
/*****************************while end*************************/
}