/*-----------------------------------------------------------*/ 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; }
/*! 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; }
//! 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; }