/*-----------------------------------------------------------*/ int init_urg_laser(urg_t **urg,int modality) { int ret; /*Switching capturing mode*/ urg_capturing_mode=modality; /*Memory structure allocation*/ *urg=malloc(sizeof(urg_t)); /*Printing and signalling allocation error*/ if (!(*urg)) { fprintf(stderr, "Error in urg memory allocation\n"); return -1; } /*Urg laser connection (structure, file descriptor, baud rate)*/ ret = urg_connect(*urg, DEVICE, 115200); if (ret < 0) { return ret; } /*Switching on the laser scanner*/ urg_laserOn(*urg); pthread_mutex_init(&mutex_laser_read, NULL); /*Getting parameters*/ if(!parameter) parameter=malloc(sizeof(urg_parameter_t)); urg_parameters(*urg, parameter); frontal_index=get_frontal_index(*urg); /*Switch to select the laser range finder acquiring mode*/ switch (urg_capturing_mode) { case CONTINUOUS: /*keep capturing*/ urg_setCaptureTimes(*urg, UrgInfinityTimes); /*request all the data*/ urg_requestData(*urg, URG_MD, URG_FIRST, URG_LAST); break; case HYBRID: /*keep capturing capture_times*/ urg_setCaptureTimes(*urg, capture_times); /*request data*/ urg_requestData(*urg, URG_MD, URG_FIRST, URG_LAST); break; case ON_DEMAND: urg_requestData(*urg, URG_GD, URG_FIRST, URG_LAST); break; default: break; } return ret; }
void checkAndConnect(Hok_t *hok) { if (!hok->isWorking) { printf("%sHokuyo not connected, trying to connect to %s\n", PREFIX, hok->path); int error = urg_connect(hok->urg, hok->path, 115200); if (error < 0) { printf("%sCan't connect to hokuyo : %s\n", PREFIX, urg_error(hok->urg)); hok->isWorking = 0; } else { hok->imin = urg_rad2index(hok->urg, hok->cone_min); hok->imax = urg_rad2index(hok->urg, hok->cone_max); urg_setCaptureTimes(hok->urg, UrgInfinityTimes); error = urg_requestData(hok->urg, URG_MD, hok->imin, hok->imax); if (error < 0) { printf("%sCan't connect to hokuyo\n", PREFIX); hok->isWorking = 0; } else { printf("%sHokuyo connected\n", PREFIX); hok->isWorking = 1; printf("%sRequesting data on indexes %d to %d from %s OK\n", PREFIX, hok->imin, hok->imax, hok->path); hok->nb_data = urg_dataMax(hok->urg); double *angles = malloc(hok->nb_data * sizeof(double)); int i; for (i=0; i<hok->nb_data; i++) { angles[i] = modTwoPi(urg_index2rad(hok->urg, i) + hok->orientation); } hok->fm = initFastmath(hok->nb_data, angles); free(angles); printf("%sCalculted sin/cos data for %s\n", PREFIX, hok->path); } } } }
/* Function: mdlOutputs ======================================================= * */ static void mdlOutputs(SimStruct *S, int_T tid) { int32_T *y0 = (int32_T *)ssGetOutputPortRealSignal(S,0); // Request Data ret = urg_requestData(&urg, URG_GD, 0, 1080); if (ret < 0) { urg_exit(&urg, "urg_requestData()"); } // Receive Data n = urg_receiveData(&urg, data, 1080); // Error, can't receive negative data points if (n < 0) { urg_exit(&urg, "urg_receiveData()"); // Disconnect Lidar urg_disconnect(&urg); } // Data Received, Construct Partial Image else{ for(k=0;k<1080;k++){ y0[k]=data[k]; } } //lidarOnly_Outputs_wrapper(y0); printf(data); }
void Hokuyo::read(SampleBuffer *sb, cvg_long *timestampMs) { int ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST); if (ret < 0) outError("Cannot request data"); ret = urg_receiveData(&urg, sb->getDataPtr(), sb->getMaxLength()); if (ret < 0) outError("Cannot receive data"); sb->setCurrentLength(ret); if (timestampMs != NULL) (*timestampMs) = urg_recentTimestamp(&urg); }
/*-----------------------------------------------------------*/ int read_laser_data(urg_t *urg) { int n; /*If there was an error...*/ /*Getting the maximum amount of data*/ data_max = urg_dataMax(urg); if (data_max<0) return -1; pthread_mutex_lock(&mutex_laser_read); if(!data_laser) data_laser=malloc(sizeof(long)*data_max); //free(data); /*Data buffer allocation (deallocation is demanded to library user)*/ /*Printing and signalling allocation error*/ if (!(data_laser)) { fprintf(stderr, "Error in data memory allocation\n"); return -1; } /*Check if capturing mode is HYBRID. If it is true, check the remaining capture times, clean the serial and request data again.*/ else { if(urg_capturing_mode==HYBRID) { /*Escamotage to avoid the laser stuck bug*/ if(urg_remainCaptureTimes(urg)<10) { urg_disconnect(urg); init_urg_laser(&urg,HYBRID); } } } /*Receive the data in the buffer*/ n = urg_receiveData(urg,data_laser,data_max); pthread_mutex_unlock(&mutex_laser_read); /*When the on_demand mode is on (it is necessary to explicitly request data)*/ if(urg_capturing_mode==ON_DEMAND) { int ret = urg_requestData(urg, URG_GD, URG_FIRST, URG_LAST); if (ret < 0) { return ret; } } /*If there was an error...*/ if (n < 0) { return -1; } return n; }
// Buffer latest measurements from device void LaserRangeFinder::update() { if (urg_requestData(& m_handle, URG_GD, URG_FIRST, URG_LAST) < 0) throw lrf_error(LRF_DATA_RETRIEVAL_FAILURE) ; m_retsiz = urg_receiveData(& m_handle, m_buffer, m_bufsiz) ; if (m_retsiz < 0) throw lrf_error(LRF_DATA_RETRIEVAL_FAILURE) ; const int m = m_angle_range.min() ; const int M = m_angle_range.max() ; for (int angle = m, i = 0; angle <= M; ++angle, ++i) { long d = m_buffer[urg_deg2index(const_cast<urg_t*>(& m_handle), angle)] ; if (d < m_distance_range.min() || d > m_distance_range.max()) d = -1 ; m_distances[i] = static_cast<int>(d) ; } }
/* Function: mdlOutputs ======================================================= * */ static void mdlOutputs(SimStruct *S, int_T tid) { const int32_T *potVal = (const int32_T*) ssGetInputPortSignal(S,0); const real_T *Speed = (const real_T*) ssGetInputPortSignal(S,1); const real_T *Upos = (const real_T*) ssGetInputPortSignal(S,2); const real_T *Dpos = (const real_T*) ssGetInputPortSignal(S,3); real_T *lidarData = (real_T *)ssGetOutputPortRealSignal(S,0); //Request data urg_setCaptureTimes(&urg, 65); ret = urg_requestData(&urg, URG_MD, 0, 1080); if (ret < 0) { urg_exit(&urg, "urg_requestData()"); } //Tell stepper to move to new position CPhidgetStepper_setTargetPosition(stepper, 0, *Dpos); for (j=0;j<65;j++) { // Receive Data n = urg_receiveData(&urg, data, 1080); // Error, can't receive negative data points if (n < 0) { urg_exit(&urg, "urg_receiveData()"); // Disconnect Lidar urg_disconnect(&urg); } // Data Received, Construct Partial Image else{ for(k=0;k<1080;k++){ lidarData[(j*1080)+k] = data[k]; } } } stepperMove(*Upos, stepper); //lidar_zero_test_Outputs_wrapper(potVal, Speed, Upos, Dpos, lidarData); }
//获取原始数据,这里去调用SCIP lib的库函数,填充DataFrame实体 bool CSCIPRadarComm::GetRawData(DataFrame * pData) { int data_max; long *data; int ret; int n; data_max = urg_dataMax(&urg); data = new long[data_max]; if (data == NULL) { //perror("malloc"); return false; } memset(data,0,sizeof(long)*data_max); ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST); if (ret < 0) { delete data; return false; } n = urg_receiveData(&urg, data, data_max);//n 表示返回的距离个数,最大1081 if (n < 0) { delete data; return false; } if( ResolveData(data,n,pData)) //解析原始数据,存入自己定义的原始数据 { delete data; return true; } else { delete data; return false; } }
/*! main */ int main(int argc, char *argv[]) { enum { CaptureTimes = 10, }; #ifdef WINDOWS_OS const char device[] = "COM3"; /* For Windows */ #else const char device[] = "/dev/ttyACM0"; /* For Linux */ #endif int data_max; long* data; urg_parameter_t parameter; int ret; int n; int i; /* Connection */ urg_t urg; ret = urg_connect(&urg, device, 115200); if (ret < 0) { urg_exit(&urg, "urg_connect()"); } /* Request for Data using GD */ data_max = urg_dataMax(&urg); data = (long*)malloc(sizeof(long) * data_max); if (data == NULL) { perror("data buffer"); exit(1); } urg_parameters(&urg, ¶meter); /* Request for Data using GD */ printf("GD capture\n"); for (i = 0; i < CaptureTimes; ++i) { /* Request for data */ ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST); if (ret < 0) { urg_exit(&urg, "urg_requestData()"); } /* Reception */ n = urg_receiveData(&urg, data, data_max); if (n < 0) { urg_exit(&urg, "urg_receiveData()"); } /* Display */ printData(&urg, ¶meter, data); } printf("\n"); /* Request for Data using MD */ printf("MD capture\n"); /* set data acquisition frequency equal to infinity, to get the data more than 100 times */ /* urg_setCaptureTimes(&urg, UrgInfinityTimes); */ assert(CaptureTimes < 100); urg_setCaptureTimes(&urg, CaptureTimes); /* Request for data */ ret = urg_requestData(&urg, URG_MD, URG_FIRST, URG_LAST); if (ret < 0) { urg_exit(&urg, "urg_requestData()"); } for (i = 0; i < CaptureTimes; ++i) { /* Reception */ n = urg_receiveData(&urg, data, data_max); if (n < 0) { urg_exit(&urg, "urg_receiveData()"); } /* Display */ printData(&urg, ¶meter, data); } urg_disconnect(&urg); free(data); #ifdef MSC getchar(); #endif return 0; }
/*! main */ int main(int argc, char *argv[]) { enum { Times = 10, Urgs = 2, }; urg_t urg[Urgs]; long *data[Urgs]; int data_max[Urgs]; int timestamp; int ret; int n; int i; int k; #ifdef WINDOWS_OS const char *devices[] = { "COM3", "COM4" }; /* For Windows */ #else const char *devices[] = { "/dev/ttyACM0", "/dev/ttyACM1" }; /* For Linux */ #endif /* Connection */ for (i = 0; i < Urgs; ++i) { urg_initialize(&urg[i]); ret = urg_connect(&urg[i], devices[i], 115200); if (ret < 0) { urg_exit(&urg[i], "urg_connect()"); } /* To clear existing MD command*/ urg_laserOff(&urg[i]); /* It will become easy if some frames are skipped. */ /* If specified skip is 2, then transferred data becomes half. */ /* urg_setSkipLines(&urg[i], 2); */ /* Reserve for receive buffer */ data_max[i] = urg_dataMax(&urg[i]); data[i] = (long*)malloc(sizeof(long) * data_max[i]); if (data[i] == NULL) { perror("data buffer"); exit(1); } } /* Request for MD data */ for (i = 0; i < Urgs; ++i) { urg_setCaptureTimes(&urg[i], Times); /* Request for data */ ret = urg_requestData(&urg[i], URG_MD, URG_FIRST, URG_LAST); if (ret < 0) { urg_exit(&urg[i], "urg_requestData()"); } } for (k = 0; k < Times; ++k) { for (i = 0; i < Urgs; ++i) { /* Ends when data reception is completed */ int remain_times = urg_remainCaptureTimes(&urg[i]); printf(" %d: ", i); printf("(%03d/%03d): ", remain_times, Times); if (remain_times <= 0) { printf("\n"); continue; } /* Reception */ n = urg_receiveData(&urg[i], data[i], data_max[i]); if (n < 0) { /* Continue processing, because there is chances of receiving the data next time. */ printf("%s: %s\n", "urg_receiveData()", urg_error(urg)); } else { /* Display */ timestamp = urg_recentTimestamp(&urg[i]); printf("timestamp: %d, ", timestamp); #if 0 { int j; for (j = 0; j < n; ++j) { /* Neglect if distance data is less than urg_minDistance() */ printf("%d:%ld, ", j, data[i][j]); } printf("\n"); } #endif printf("\n"); } } } /* Disconnect */ for (i = 0; i < Urgs; ++i) { urg_disconnect(&urg[i]); free(data[i]); } #ifdef MSC getchar(); #endif return 0; }
/* Function: mdlOutputs ======================================================= * */ static void mdlOutputs(SimStruct *S, int_T tid) { const int32_T *potVal = (const int32_T*) ssGetInputPortSignal(S,0); const real_T *Speed = (const real_T*) ssGetInputPortSignal(S,1); const real_T *Upos = (const real_T*) ssGetInputPortSignal(S,2); const real_T *Dpos = (const real_T*) ssGetInputPortSignal(S,3); const real_T *ScanNumber = (const real_T*) ssGetInputPortSignal(S,4); real_T *lidarData = (real_T *)ssGetOutputPortRealSignal(S,0); real_T *udOut = (real_T *)ssGetOutputPortRealSignal(S,1); //Read number of scans int ScanNum = *ScanNumber; CPhidgetStepper_setVelocityLimit(stepper, 0, *Speed); //Variables required for zeroing //Calibrated for three turn pot int zeroPos = 517; int dpot; int dstep; //Pot reader may take a minute to initialize and will send zero //If this is the case do nothing if (*potVal == 0) {} // Otherwise scan else { //Zero stepper on first run //Account for differnces coming from different directions if (counter == 0) { if (zeroPos > *potVal) { zeroPos = 515; } if (zeroPos < *potVal) { zeroPos = 519; } //Find required steps and move to zero position printf("potVal:%d\n", *potVal); dpot = zeroPos - *potVal; printf("Dpot:%d\n", dpot); dstep = -1036/125*dpot; //If error occurs dstep may be very large //Limit dstep value to prevent damage to system if (dstep > 1600) { dstep = 1600; } if (dstep < -1600) { dstep = -1600; } printf("Dstep:%d\n", dstep); stepperMove(dstep, stepper); //Set current position to zero CPhidgetStepper_setCurrentPosition(stepper, 0, 0); //Move to Up position to begin scanning process stepperMove(*Upos, stepper); counter = 1; } //Request data urg_setCaptureTimes(&urg, ScanNum); ret = urg_requestData(&urg, URG_MD, 0, 1080); if (ret < 0) { urg_exit(&urg, "urg_requestData()"); } //Tell stepper to move to new position if (counter == 1) { CPhidgetStepper_setTargetPosition(stepper, 0, *Dpos); counter = 2; *udOut = 0; } else if (counter == 2) { CPhidgetStepper_setTargetPosition(stepper, 0, *Upos); counter = 1; *udOut = 1; } for (j=0;j<ScanNum;j++) { // Receive Data n = urg_receiveData(&urg, data, 1080); // Error, can't receive negative data points if (n < 0) { urg_exit(&urg, "urg_receiveData()"); // Disconnect Lidar urg_disconnect(&urg); } // Data Received, Construct Partial Image else{ for(k=0;k<1080;k++){ lidarData[(j*1080)+k] = data[k]; } } } //stepperMove(*Upos, stepper); } //lidar_zero_test_v4_Outputs_wrapper(potVal, Speed, Upos, Dpos, ScanNumber, lidarData, udOut); }
int main(int argc, char *argv[]) { #ifdef WINDOWS_OS const char device[] = "COM3"; /* For Windows */ #else const char device[] = "/dev/ttyACM0"; /* For Linux */ #endif int data_max; long *data; int timestamp; int ret; int n; int i; /* Connection */ urg_t urg; urg_initialize(&urg); ret = urg_connect(&urg, device, 115200); if (ret < 0) { urg_exit(&urg, "urg_connect()"); } /* Reserve for reception data */ data_max = urg_dataMax(&urg); data = (long*)malloc(sizeof(long) * data_max); if (data == NULL) { perror("malloc"); exit(1); } /* Request for GD data */ ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST); if (ret < 0) { urg_exit(&urg, "urg_requestData()"); } /* Reception */ n = urg_receiveData(&urg, data, data_max); printf("# n = %d\n", n); if (n < 0) { urg_exit(&urg, "urg_receiveData()"); } /* Display */ timestamp = urg_recentTimestamp(&urg); printf("# timestamp: %d\n", timestamp); for (i = 0; i < n; ++i) { /*Neglect the distance less than urg_minDistance() */ printf("%d %ld, ", i, data[i]); } printf("\n"); urg_disconnect(&urg); free(data); #ifdef MSC getchar(); #endif return 0; }
/*! main */ int main(int argc, char *argv[]) { #ifdef WINDOWS_OS const char device[] = "COM3"; /* For Windows */ #else const char device[] = "/dev/ttyACM0"; /* For Linux */ #endif long *data = NULL; int data_max; int min_length = 0; int max_length = 0; int ret; int n; int i; /* Connection */ urg_t urg; urg_initialize(&urg); ret = urg_connect(&urg, device, 115200); if (ret < 0) { urg_exit(&urg, "urg_connect()"); } /* Reserve for Reception data */ data_max = urg_dataMax(&urg); data = (long*)malloc(sizeof(long) * data_max); if (data == NULL) { perror("data buffer"); exit(1); } /* Request for GD data */ ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST); if (ret < 0) { urg_exit(&urg, "urg_requestData()"); } /* Reception */ n = urg_receiveData(&urg, data, data_max); if (n < 0) { urg_exit(&urg, "urg_receiveData()"); } /* Output as 2 dimensional data */ /* Consider front of URG as positive direction of X axis */ min_length = urg_minDistance(&urg); max_length = urg_maxDistance(&urg); for (i = 0; i < n; ++i) { int x, y; long length = data[i]; /* Neglect the out of range values */ if ((length <= min_length) || (length >= max_length)) { continue; } x = (int)(length * cos(urg_index2rad(&urg, i))); y = (int)(length * sin(urg_index2rad(&urg, i))); printf("%d\t%d\t# %d, %ld\n", x, y, i, length); } urg_disconnect(&urg); free(data); #ifdef MSC getchar(); #endif return 0; }
/* Function: mdlOutputs ======================================================= * */ static void mdlOutputs(SimStruct *S, int_T tid) { const int32_T *potVal = (const int32_T*) ssGetInputPortSignal(S,0); const real_T *Speed = (const real_T*) ssGetInputPortSignal(S,1); const real_T *Upos = (const real_T*) ssGetInputPortSignal(S,2); const real_T *Dpos = (const real_T*) ssGetInputPortSignal(S,3); const real_T *ScanNumber = (const real_T*) ssGetInputPortSignal(S,4); real_T *lidarData = (real_T *)ssGetOutputPortRealSignal(S,0); uint8_T *Red = (uint8_T *)ssGetOutputPortRealSignal(S,1); uint8_T *Green = (uint8_T *)ssGetOutputPortRealSignal(S,2); uint8_T *Blue = (uint8_T *)ssGetOutputPortRealSignal(S,3); real_T *udOut = (real_T *)ssGetOutputPortRealSignal(S,4); //Read number of scans int ScanNum = *ScanNumber; CPhidgetStepper_setVelocityLimit(stepper, 0, *Speed); //Parameters required for zeroing //Calibrated for three turn pot int zeroPos = 534; //Potentiometer reading at level position int stepRes = 3950; //Number of steps to move the stepper 180 degrees int potRes = 323; //Number of potValues that represent the rotation of the gimbal 180 degrees int potGive = 2; //Accounts for slack in gearing for pot //Variables required for zeroing int dpot; //difference to desired pot value from current int dstep; //number of steps the stepper must move to get to desired position //Pot reader may take a minute to initialize and will send zero //If this is the case do nothing if (*potVal != 0){ //Zero stepper on first run //Account for differnces coming from different directions if (counter == 0) { //write pot value printf("potVal:%d\n", *potVal); //Find pot value difference to calculate steps needed to level if (zeroPos > *potVal) { dpot = zeroPos - potGive - *potVal; } if (zeroPos < *potVal) { dpot = zeroPos + potGive -*potVal; } //write difference printf("Dpot:%d\n", dpot); //Find required steps and move to level position dstep = -stepRes/potRes*dpot; //If error occurs dstep may be very large //Limit dstep value to prevent damage to system if (dstep > 1600) { dstep = 1600; } if (dstep < -1600) { dstep = -1600; } //write number of steps to zero printf("Dstep:%d\n", dstep); stepperMove(dstep, stepper); //Set current position to zero CPhidgetStepper_setCurrentPosition(stepper, 0, 0); //Move to Up position to begin scanning process stepperMove(*Upos, stepper); counter = 1; } //Request data urg_setCaptureTimes(&urg, ScanNum); ret = urg_requestData(&urg, URG_MD, 0, 1080); if (ret < 0) { urg_exit(&urg, "urg_requestData()"); } //Tell stepper to move to new position if (counter == 1) { CPhidgetStepper_setTargetPosition(stepper, 0, *Dpos); counter = 2; *udOut = 0; } else if (counter == 2) { CPhidgetStepper_setTargetPosition(stepper, 0, *Upos); counter = 1; *udOut = 1; } for (j=0;j<ScanNum;j++) { // Receive Data n = urg_receiveData(&urg, data, 1080); // Error, can't receive negative data points if (n < 0) { urg_exit(&urg, "urg_receiveData()"); // Disconnect Lidar urg_disconnect(&urg); } // Data Received, Construct Partial Image else{ for(k=0;k<1080;k++){ lidarData[(j*1080)+k] = data[k]; } } } }// END OF LIDAR DATA ACQ // START CAM ACQ // Grab a number of frames in order to flush the data buffer int flush1 = cvGrabFrame( capture1 ); int flush2 = cvGrabFrame( capture1 ); int flush3 = cvGrabFrame( capture1 ); int flush4 = cvGrabFrame( capture1 ); IplImage *frame1 = cvQueryFrame( capture1 ); //Gets Frame from Camera unsigned char *data1 = frame1->imageData; long iNew, jNew = 0, kNew = 0; for(iNew = 0; iNew < 480*640*3; iNew+=3){ //printf("data: %d, %d, %d \n",data[i],data[i+1], data[i+2]); Blue[jNew*480+kNew] = data1[iNew]; Green[jNew*480+kNew] = data1[iNew+1]; Red[jNew*480+kNew] = data1[iNew+2]; jNew++; if(jNew==640){kNew++; jNew = 0;} } }
/* Function: mdlOutputs ======================================================= * */ static void mdlOutputs(SimStruct *S, int_T tid) { const int32_T *choice = (const int32_T*) ssGetInputPortSignal(S, 0); const int32_T *potVal = (const int32_T*) ssGetInputPortSignal(S, 1); const real_T *Speed = (const real_T*) ssGetInputPortSignal(S, 2); const real_T *Upos = (const real_T*) ssGetInputPortSignal(S, 3); const real_T *Dpos = (const real_T*) ssGetInputPortSignal(S, 4); const real_T *ScanNumber = (const real_T*) ssGetInputPortSignal(S, 5); real_T *lidarData = (real_T *)ssGetOutputPortRealSignal(S, 0); uint8_T *Red = (uint8_T *)ssGetOutputPortRealSignal(S, 1); uint8_T *Green = (uint8_T *)ssGetOutputPortRealSignal(S, 2); uint8_T *Blue = (uint8_T *)ssGetOutputPortRealSignal(S, 3); real_T *udOut = (real_T *)ssGetOutputPortRealSignal(S, 4); //Read number of scans int ScanNum = *ScanNumber; IplImage *frame1; CPhidgetStepper_setVelocityLimit(stepper, 0, *Speed); //Parameters required for zeroing //Calibrated for three turn pot int zeroPos = 483; //Potentiometer reading at level position int stepRes = 7328; //Number of steps to move the gimbal 180 degrees int potRes = 741; //Number of potValues that represent the rotation of the gimbal 180 degrees int potGive = 14; //Accounts for slack in gearing for pot //Variables required for zeroing int dpot; //difference to desired pot value from current int dstep; //number of steps the stepper must move to get to desired position int midscan = ScanNum/2; // Stepper Always On // Option 0 - Lidar Only // Option 1 - Cam Only // Option 2 - All On // Default case - Do Nothing/Error Recovery switch(*choice){ ////////////////////////////////////////////////////// /////////////// Option 0 - Lidar Only /////////////// ////////////////////////////////////////////////////// case 0:{ //Pot reader may take a minute to initialize and will send zero //If this is the case do nothing if (*potVal != 0) { //Zero stepper on first run //Account for differnces coming from different directions if (counter == 0) { printf("potVal:%d\n", *potVal); //Find pot value difference to calculate steps needed to level if (zeroPos > *potVal) { dpot = zeroPos - potGive - *potVal; } else if (zeroPos < *potVal) { dpot = zeroPos + potGive -*potVal; } else { dpot = 0; } printf("Dpot:%d\n", dpot); //Find required steps and move to level position dstep = -stepRes/potRes*dpot; //If error occurs dstep may be very large //Limit dstep value to prevent damage to system if (dstep > 3000) dstep = 3000; if (dstep < -3000) dstep = -3000; printf("Dstep:%d\n", dstep); stepperMove(dstep, stepper); //Set current position to zero CPhidgetStepper_setCurrentPosition(stepper, 0, 0); //Move to Up position to begin scanning process stepperMove(*Upos, stepper); counter = 1; } if (lidarCounter == 0) { camCounter = 0; bothCounter = 0; lidarCounter = 1; } //Tell stepper to move to new position if (counter == 1) { CPhidgetStepper_setTargetPosition(stepper, 0, *Dpos); counter = 2; *udOut = 0; } else if (counter == 2) { CPhidgetStepper_setTargetPosition(stepper, 0, *Upos); counter = 1; *udOut = 1; } //Sleep to allow stepper to start moving usleep(120000); //Request data urg_setCaptureTimes(&urg, ScanNum); ret = urg_requestData(&urg, URG_MD, 0, 1080); if (ret < 0) urg_exit(&urg, "urg_requestData()"); for (j=0;j<ScanNum;j++) { // Receive Data n = urg_receiveData(&urg, data, 1080); // Error, can't receive negative data points if (n < 0) { urg_exit(&urg, "urg_receiveData()"); // Disconnect Lidar urg_disconnect(&urg); } // Data Received, Construct Partial Image else{ for(k=0;k<1080;k++){ lidarData[(j*1080)+k] = data[k]; } } } }// END OF LIDAR DATA ACQ break; } /////////////////////////////////////////////////// /////////////// Option 1 - Cam Only /////////////// /////////////////////////////////////////////////// case 1:{ if (camCounter == 0) { // Grab a number of frames in order to flush the data buffer int flush1 = cvGrabFrame( capture1 ); int flush2 = cvGrabFrame( capture1 ); int flush3 = cvGrabFrame( capture1 ); int flush4 = cvGrabFrame( capture1 ); lidarCounter = 0; bothCounter = 0; camCounter = 1; } frame1 = cvQueryFrame( capture1 ); //Gets Frame from Camera unsigned char *data1 = frame1->imageData; long iNew, jNew = 0, kNew = 0; for(iNew = 0; iNew < 480*640*3; iNew+=3){ //printf("data: %d, %d, %d \n",data[i],data[i+1], data[i+2]); Blue[jNew*480+kNew] = data1[iNew]; Green[jNew*480+kNew] = data1[iNew+1]; Red[jNew*480+kNew] = data1[iNew+2]; jNew++; if(jNew==640){kNew++; jNew = 0;} } break; } /////////////////////////////////////////////////// //////////////// Option 2 - All On /////////////// /////////////////////////////////////////////////// case 2:{ //Pot reader may take a minute to initialize and will send zero //If this is the case do nothing //Zero stepper on first run //Account for differnces coming from different directions if (counter == 0) { printf("potVal:%d\n", *potVal); //Find pot value difference to calculate steps needed to level if (zeroPos > *potVal) { dpot = zeroPos - potGive - *potVal; } else if (zeroPos < *potVal) { dpot = zeroPos + potGive -*potVal; } else { dpot = 0; } printf("Dpot:%d\n", dpot); //Find required steps and move to level position dstep = -stepRes/potRes*dpot; //If error occurs dstep may be very large //Limit dstep value to prevent damage to system if (dstep > 3000) dstep = 3000; if (dstep < -3000) dstep = -3000; printf("Dstep:%d\n", dstep); stepperMove(dstep, stepper); //Set current position to zero CPhidgetStepper_setCurrentPosition(stepper, 0, 0); //Move to Up position to begin scanning process stepperMove(*Upos, stepper); counter = 1; } if (bothCounter == 0) { //Preform on first run lidarCounter = 0; camCounter = 0; bothCounter = 1; } //START OF DATA ACQUISITION //Tell stepper to move to new position if (counter == 1) { CPhidgetStepper_setTargetPosition(stepper, 0, *Dpos); counter = 2; *udOut = 0; } else if (counter == 2) { CPhidgetStepper_setTargetPosition(stepper, 0, *Upos); counter = 1; *udOut = 1; } //Sleep to allow stepper to start moving usleep(120000); //Request data urg_setCaptureTimes(&urg, ScanNum); ret = urg_requestData(&urg, URG_MD, 0, 1080); if (ret < 0) urg_exit(&urg, "urg_requestData()"); //Sleep so image will be taken in middle of lidar scan //usleep(100000); // Grab a number of frames in order to flush the data buffer int flush1 = cvGrabFrame( capture1 ); int flush2 = cvGrabFrame( capture1 ); int flush3 = cvGrabFrame( capture1 ); int flush4 = cvGrabFrame( capture1 ); // Acquire from from webcam //IplImage *frame1 = cvQueryFrame( capture1 ); for (j=0;j<ScanNum;j++) { // Receive Data n = urg_receiveData(&urg, data, 1080); // Grab frame from webcam in middle of scan if (j == midscan) { frame1 = cvQueryFrame( capture1 ); } // Error, can't receive negative data points if (n < 0) { urg_exit(&urg, "urg_receiveData()"); // Disconnect Lidar urg_disconnect(&urg); } // Data Received, Construct Partial Image else{ for(k=0;k<1080;k++){ lidarData[(j*1080)+k] = data[k]; } } } unsigned char *data1 = frame1->imageData; long iNew, jNew = 0, kNew = 0; for(iNew = 0; iNew < 480*640*3; iNew+=3){ //printf("data: %d, %d, %d \n",data[i],data[i+1], data[i+2]); Blue[jNew*480+kNew] = data1[iNew]; Green[jNew*480+kNew] = data1[iNew+1]; Red[jNew*480+kNew] = data1[iNew+2]; jNew++; if(jNew==640){kNew++; jNew = 0;} } break; } ///// Default Case for all other invalid input ///// default:{ lidarCounter = 0; camCounter = 0; bothCounter = 0; break; } // END OF LIDAR DATA ACQ } }
//! main int main(int argc, char *argv[]) { enum { CaptureTimes = 10, }; #ifdef WINDOWS_OS const char device[] = "COM3"; /* For Windows */ #else const char device[] = "/dev/ttyACM0"; /* For Linux */ #endif int data_max; long* data; int timestamp = -1; int previous_timestamp; int remain_times; //int scan_msec; urg_parameter_t parameter; int ret; int n; int i; urg_t urg; /* Connection */ urg_initialize(&urg); ret = urg_connect(&urg, device, 115200); if (ret < 0) { urg_exit(&urg, "urg_connect()"); exit(1); } /* Reserve for receive buffer */ data_max = urg_dataMax(&urg); data = (long*)malloc(sizeof(long) * data_max); if (data == NULL) { fprintf(stderr, "data_max: %d\n", data_max); perror("data buffer"); exit(1); } urg_parameters(&urg, ¶meter); //scan_msec = urg_scanMsec(&urg); /* Request for MD data */ /* To get data continuously for more than 100 times, set capture times equal to infinity times(UrgInfinityTimes) */ /* urg_setCaptureTimes(&urg, UrgInfinityTimes); */ assert(CaptureTimes < 100); urg_setCaptureTimes(&urg, CaptureTimes); /* Request for data */ ret = urg_requestData(&urg, URG_MD, URG_FIRST, URG_LAST); if (ret < 0) { urg_exit(&urg, "urg_requestData()"); } for (i = 0; i < CaptureTimes; ++i) { /* Reception */ n = urg_receiveData(&urg, data, data_max); printf("n = %d\n", n); if (n < 0) { urg_exit(&urg, "urg_receiveData()"); } else if (n == 0) { printf("n == 0\n"); --i; continue; } /* Display the front data with timestamp */ /* Delay in reception of data at PC causes URG to discard the data which cannot be transmitted. This may results in remain_times to become discontinuous */ previous_timestamp = timestamp; timestamp = urg_recentTimestamp(&urg); remain_times = urg_remainCaptureTimes(&urg); /* Neglect the distance data if it is less than urg_minDistance() */ printf("%d/%d: %ld [mm], %d [msec], (%d)\n", remain_times, CaptureTimes, data[parameter.area_front_], timestamp, timestamp - previous_timestamp); printf("%d, %d\n", i, remain_times); if (remain_times <= 0) { break; } } urg_disconnect(&urg); free(data); #ifdef MSC getchar(); #endif return 0; }