int main(int argc, char *argv[]){ enum { CAPTURE_TIMES = 1, //modificat_numarul_de_afisari_ale_citirii }; urg_t urg; long *data = NULL; long time_stamp; int n; int i; if (open_urg_sensor(&urg, argc, argv) < 0) { return 1; } data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0])); if (!data) { perror("1urg_max_index()"); return 1; } // \~japanese データ取得 #if 0 // \~japanese データの取得範囲を変更する場合 urg_set_scanning_parameter(&urg, urg_deg2step(&urg, -90), urg_deg2step(&urg, +90), 0); #endif urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, 0); for (i = 0; i < CAPTURE_TIMES; ++i) { n = urg_get_distance(&urg, data, &time_stamp); if (n <= 0) { printf("urg_get_distance: %s\n", urg_error(&urg)); free(data); urg_close(&urg); return 1; } print_data(&urg, data, n, time_stamp); } // \~japanese 切断 free(data); urg_close(&urg); #if defined(URG_MSC) getchar(); #endif return 0; }
raw_sensor_scan sensor_read_raw() { // Initialize parameters for laser scanning int start_step = 44; int end_step = 725; int step_cluster = 1; int scan_interval = 0; int scan_num = 1; int step_num; long timestamp; int sleep_time = poll_time_usb - (utime() - last_poll_usb); if (sleep_time > 0) usleep(sleep_time); else printf("sleepless for %g seconds\n", -sleep_time/1000000.0); if (eth) { step_num = urg_get_distance(&connection_eth, buffer_eth, ×tamp); last_poll_eth = utime(); } if (usb) { buffer_usb = scip2MeasureScan(connection_usb, start_step, end_step, step_cluster, scan_interval, scan_num, ENC_3BYTE, &step_num); last_poll_usb = utime(); } int i, distance; for (i = 0 ; i < step_num ; i++) { distance = buffer_usb[0][i]; if ((distance >= SENSOR_MIN) && (distance <= SENSOR_MAX_USB)) // Copy the data into the static variable lidar_data.distances[i] = distance; } // free usb_buffer, it is malloc'd every time we call scip2MeasureScan for (i = 0; i < scan_num; i++) free(buffer_usb[i]); free(buffer_usb); return lidar_data; }
bool captureData(void * params, void ** data) { URGPARAMS * tempparams=(URGPARAMS *)params; urg_start_measurement(&(tempparams->urg),tempparams->measurement_type,1,tempparams->skip_scan); long urgdata[URGMAXSIZE]; long timestamp; int n=urg_get_distance(&(tempparams->urg),urgdata,×tamp); QTime qtimestamp=QTime::currentTime(); if(n<=0) { return 0; } URGDATA * tempdata; if(*data==NULL) { *data=(void *)new URGDATA; tempdata=(URGDATA *)(*data); tempdata->datasize=n; tempdata->data=new short[tempdata->datasize]; } else { tempdata=(URGDATA *)(*data); if(tempdata->datasize!=n) { delete [](tempdata->data); tempdata->datasize=n; tempdata->data=new short[tempdata->datasize]; } } tempdata->timestamp=timestamp; tempdata->qtimestamp=qtimestamp; #pragma omp for schedule(dynamic,1) for(int i=0;i<n;i++) { tempdata->data[i]=short(urgdata[i]/10); } return 1; }
bool captureData(void * params, void ** data) //capture data using params and store in data { URGSENSORPARAMS * URGparams=(URGSENSORPARAMS *)params; urg_start_measurement(&(URGparams->urg),URGparams->measurement_type,1,URGparams->skip_scan); long urgdata[URGMAXSIZE]; long timestamp; int n=urg_get_distance(&(URGparams->urg),urgdata,×tamp); QTime qtimestamp=QTime::currentTime(); if(n<=0) { return 0; } URGSENSORDATA * URGdata=(URGSENSORDATA *)(*data); if(URGdata==NULL) {//case1: data point to a NULL *data=(void *)new URGSENSORDATA; URGdata=(URGSENSORDATA *)(*data); URGdata->datasize=n; URGdata->data=new short[URGdata->datasize]; } else {//case2: URG point to a memory block if(URGdata->datasize!=n) { delete [](URGdata->data); URGdata->datasize=n; URGdata->data=new short[URGdata->datasize]; } } URGdata->timestamp=timestamp; URGdata->qtimestamp=qtimestamp; #pragma omp for schedule(dynamic,1) for(int i=0;i<n;i++) { URGdata->data[i]=short(urgdata[i]/10); } return 1; }
bool LRF::GetLRFData(ros::Time& t, sensor_msgs::LaserScan &data) { if(publish_intensity){ t = ros::Time::now(); if(urg_get_distance_intensity(&urg, &data.ranges[0], &data.intensities[0], ×tamp) <= 0){ ROS_WARN("Disable to get LRF data."); for(int i=0; i<step; i++){ data.ranges[i] = 0; data.intensities[i] = 0; } return false; } } else{ t = ros::Time::now(); if(urg_get_distance(&urg, &data.ranges[0], ×tamp) <= 0){ ROS_WARN("Disable to get LRF data."); for(int i=0; i<step; i++) data.ranges[i] = 0; return false; } } return true; }
int get_points_2d(struct urg_params urg, struct coord *points){ int data_max, n, i; long *data; data_max = urg_max_data_size(urg.ptr);//aquisition du nombre de points data = (long*)malloc(sizeof(long) * data_max); if(data == NULL){ fprintf(stderr, "data malloc error\n"); exit(1); } if(points == NULL){ fprintf(stderr, "get_points_2d : points non alloué\n"); exit(1); } n = urg_get_distance(urg.ptr, data, NULL); // CONVERSION EN COORDONNEES for(i=0; i<n; i++){ /* double rad = urg_index2rad(urg.ptr, i); points[i].x = data[i]*cos(rad) - urg.x; //x points[i].y = data[i]*sin(rad) - urg.y; //y */ points[i].x = data[i]*get_val(cosinus, i, urg.ptr) + urg.x; //x points[i].y = data[i]*get_val(sinus, i, urg.ptr) + urg.y; //y if(urg.side == GREEN_SIDE){//si blue side, symétrie centrale points[i].x = LX - points[i].x; points[i].y = LY - points[i].y; } } free(data);//free raw datas return n; }
int main(int argc, char *argv[]) { enum { CAPTURE_TIMES = 1, }; urg_t urg; urg_connection_type_t connection_type = URG_SERIAL; long *data = NULL; long time_stamp; int n; int i; #if defined(URG_WINDOWS_OS) const char *device = "COM3"; #elif defined(URG_LINUX_OS) const char *device = "/dev/ttyACM0"; #else #endif long baudrate_or_port = 115200; const char *ip_address = "192.168.0.10"; // 接続タイプの切替え for (i = 1; i < argc; ++i) { if (!strcmp(argv[i], "-e")) { connection_type = URG_ETHERNET; baudrate_or_port = 10940; device = ip_address; } } for (i = 1; i < argc; ++i) { if (!strcmp(argv[i], "-e")) { connection_type = URG_ETHERNET; } } // \~japanese 接続 if (urg_open(&urg, connection_type, device, baudrate_or_port) < 0) { printf("urg_open: %s\n", urg_error(&urg)); return 1; } data = (long *)malloc(urg_max_data_size(&urg) * 3 * sizeof(data[0])); if (!data) { perror("urg_max_index()"); return 1; } // \~japanese データ取得 urg_start_measurement(&urg, URG_MULTIECHO, CAPTURE_TIMES, 0); for (i = 0; i < CAPTURE_TIMES; ++i) { n = urg_get_distance(&urg, data, &time_stamp); if (n <= 0) { printf("urg_distance: %s\n", urg_error(&urg)); free(data); urg_close(&urg); return 1; } print_data(&urg, data, n, time_stamp); } // \~japanese 切断 free(data); urg_close(&urg); #if defined(URG_MSC) getchar(); #endif return 0; }