void LRF::OpenLRF(sensor_msgs::LaserScan &data) { data.angle_min = deg_min*M_PI/180.0; data.angle_max = deg_max*M_PI/180.0; data.range_min = range_min; data.range_max = range_max; data.angle_increment = (skip+1)/1080.0*270.0*M_PI/180.0; step = (deg_max - deg_min)*4.0 + 1; data.ranges.resize(step); data.intensities.resize(step); if(eth_flg){ if(urg_open(&urg,URG_ETHERNET,ip_address.c_str(),(long)ip_port) < 0){ ROS_WARN("Ethernet LRF open error."); throw runtime_error("Ethernet LRF open error."); } } else{ if(urg_open(&urg,URG_SERIAL,serial_port.c_str(),(long)serial_baud) < 0){ ROS_WARN("Serial LRF open error."); throw runtime_error("Serial LRF open error."); } } urg_set_scanning_parameter(&urg, urg_deg2step(&urg,deg_min), urg_deg2step(&urg,deg_max), skip); if(publish_intensity) urg_start_measurement(&urg, URG_DISTANCE_INTENSITY, URG_SCAN_INFINITY, skip); else urg_start_measurement(&urg, URG_DISTANCE, URG_SCAN_INFINITY, skip); }
int main(int argc, char **argv) { urg_t connection; urg_connection_type_t type = URG_ETHERNET; char *device = "192.168.0.10"; int direction_count, max_data_size, i; long timestamp; long *data; // open connection if (urg_open(&connection, type, device, 10940) < 0) return 1; max_data_size = urg_max_data_size(&connection); data = malloc(sizeof(long)*max_data_size); // start measurement // (connection, type, scan times (0 is keep going), skip) urg_start_measurement(&connection, type, 0, 0); while (1) { direction_count = urg_get_distance(&connection, data, ×tamp); for (i = 0; i < max_data_size; i++) { printf("%i ", data[i]); } printf("\nmax_data_size: %i\n", max_data_size); sleep(1); } return 0; }
int main() { lidar_init(); static int k = 1; int i; for (i = 0; i < 100; i++) { lidar_fp = fopen("/root/lidarProfileTest", "w"); if (urg_start_measurement(&lidar, URG_DISTANCE, 1, 0) < 0) { printf("ERROR: unable to start measurement on LIDAR\n"); } length_data_size = urg_get_distance(&lidar, length_data, NULL); if (length_data_size <= 0) { printf("ERROR: unable to get distance data from LIDAR\n"); } fprintf(lidar_fp, "%i\t%0.2f", k++, 1.00); int i; for (i = 0; i < length_data_size; i++) { fprintf(lidar_fp, "\t%ld", length_data[i]); } fprintf(lidar_fp, "\n"); fclose(lidar_fp); } return 0; }
int init(urg_t *urg){ int error; int i; char *device = "/dev/ttyACM0"; //SERIAL PORT DETECTION fprintf(stderr, "List of serial ports :\n"); int found_port_size = urg_serial_find_port(); if (found_port_size == 0) { fprintf(stderr, "could not found serial ports.\n"); exit(EXIT_FAILURE); } for (i = 0; i < found_port_size; ++i) { fprintf(stderr, "%s", (char *)urg_serial_port_name(i)); device = (char *)urg_serial_port_name(i); } fprintf(stderr, "\n"); fprintf(stderr, "Connection à %s\n", device); error = urg_open(urg, URG_SERIAL, device, BAUDRATE); if(error < 0){ error_func(urg, "connection failed"); } else{ fprintf(stderr, "Connection établie à %s\n", device); urg_set_scanning_parameter(urg, urg_rad2step(urg, ANGLE_MIN), urg_rad2step(urg, ANGLE_MAX), 0);//scan en continu, on ne garde que les point entre -PI/2 et PI/2 fprintf(stderr, "Parameters set\n"); error = urg_start_measurement(urg, URG_DISTANCE, URG_SCAN_INFINITY, 0); if(error < 0){ error_func(urg, "failed to start measurement"); } } get_val(calc, 0, urg);//calcule les tables de cos/sin à l'avance return error; }
int main(void) { urg_t urg; long *length_data = NULL; int ret; // \~japanese タイムスタンプの取得 // \~english Gets timestamp values // \~japanese urg_get_distance() 関数に変数を与え、タイムスタンプを取得する。 // \~english Uses the urg_get_distance() function and returns the timestamp values for each scan const int scan_times = 123; int length_data_size; long timestamp; int i; // \~japanese センサから距離データを取得する。 // \~english Starts range data measurement ret = urg_start_measurement(&urg, URG_DISTANCE, scan_times, 0); // \todo check error code for (i = 0; i < scan_times; ++i) { length_data_size = urg_get_distance(&urg, length_data, ×tamp); // \todo process length_data array // \~japanese 取得したタイムスタンプを出力する // \~english Outputs the received timestamp value printf("%ld\n", timestamp); } return 0; }
int main(void) { const char connect_device[] = "/dev/ttyACM0"; const long connect_baudrate = 115200; urg_t urg; int first_step; int last_step; int skip_step; int scan_times; int skip_scan; int ret; // 計測パラメータの設定 // センサに対して接続を行う。 // 接続を行うと、計測パラメータの設定は初期化される ret = urg_open(&urg, URG_SERIAL, connect_device, connect_baudrate); // \todo check error code // 計測範囲を指定する // センサ正面方向の 90 [deg] 範囲のデータ取得を行い、ステップ間引きを行わない例 first_step = urg_rad2step(&urg, -45); last_step = urg_rad2step(&urg, +45); skip_step = 0; ret = urg_set_scanning_parameter(&urg, first_step, last_step, skip_step); // \todo check error code // 計測回数と計測の間引きを指定して、計測を開始する // 123 回の計測を指示し、スキャンの間引きを行わない例 scan_times = 123; skip_scan = 0; ret = urg_start_measurement(&urg, URG_DISTANCE, scan_times, skip_scan); // \todo check error code return 0; }
int main(int argc, char *argv[]) { urg_t urg; long *data; long max_distance; long min_distance; long time_stamp; int i; int n; if (open_urg_sensor(&urg, argc, argv) < 0) { return 1; } data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0])); if (!data) { perror("urg_max_index()"); return 1; } // \~japanese データ取得 urg_start_measurement(&urg, URG_DISTANCE, 1, 0); n = urg_get_distance(&urg, data, &time_stamp); if (n < 0) { printf("urg_get_distance: %s\n", urg_error(&urg)); urg_close(&urg); return 1; } // \~japanese X-Y 座標系の値を出力 urg_distance_min_max(&urg, &min_distance, &max_distance); for (i = 0; i < n; ++i) { long distance = data[i]; double radian; long x; long y; if ((distance < min_distance) || (distance > max_distance)) { continue; } radian = urg_index2rad(&urg, i); x = (long)(distance * cos(radian)); y = (long)(distance * sin(radian)); printf("%ld, %ld\n", x, y); } printf("\n"); // \~japanese 切断 free(data); urg_close(&urg); #if defined(URG_MSC) getchar(); #endif return 0; }
int main(int argc, char *argv[]) { enum { CAPTURE_TIMES = 0, }; 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("urg_max_index()"); return 1; } // \~japanese データ取得 #if 0 // \~japanese データの取得範囲を変更する場合 urg_set_scanning_parameter(&urg, urg_deg2step(&urg, -90), urg_deg2step(&urg, +90), 0); #endif int k; for (k = 0; k < 3; k++) { urg_start_measurement(&urg, URG_DISTANCE, CAPTURE_TIMES, 0); for (i = 0; i < 25; ++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); } urg_stop_measurement(&urg); sleep(10); } // \~japanese 切断 free(data); urg_close(&urg); #if defined(URG_MSC) getchar(); #endif return 0; }
int main(int argc, char *argv[]) { enum { CAPTURE_TIMES = 10, }; urg_t urg; int max_data_size; long *data = NULL; unsigned short *intensity = NULL; long time_stamp; int n; int i; if (open_urg_sensor(&urg, argc, argv) < 0) { return 1; } max_data_size = urg_max_data_size(&urg); data = (long *)malloc(max_data_size * sizeof(data[0])); if (!data) { perror("urg_max_index()"); return 1; } intensity = malloc(max_data_size * sizeof(intensity[0])); if (!intensity) { perror("urg_max_index()"); return 1; } // データ取得 urg_start_measurement(&urg, URG_DISTANCE_INTENSITY, URG_SCAN_INFINITY, 0); for (i = 0; i < CAPTURE_TIMES; ++i) { n = urg_get_distance_intensity(&urg, data, intensity, &time_stamp); if (n <= 0) { printf("urg_get_distance_intensity: %s\n", urg_error(&urg)); free(data); urg_close(&urg); return 1; } print_data(&urg, data, intensity, n, time_stamp); } // 切断 free(intensity); free(data); urg_close(&urg); #if defined(URG_MSC) getchar(); #endif return 0; }
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; }
int main(int argc, char *argv[]) { enum { CAPTURE_TIMES = 10, }; 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) * 3 * sizeof(data[0])); if (!data) { perror("urg_max_index()"); return 1; } // データ取得 urg_start_measurement(&urg, URG_MULTIECHO, URG_SCAN_INFINITY, 0); for (i = 0; i < CAPTURE_TIMES; ++i) { n = urg_get_multiecho(&urg, data, &time_stamp); if (n <= 0) { printf("urg_get_multiecho: %s\n", urg_error(&urg)); free(data); urg_close(&urg); return 1; } print_data(&urg, data, n, time_stamp); } // 切断 free(data); urg_close(&urg); #if defined(URG_MSC) getchar(); #endif return 0; }
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; }
void* sensor_init(void *null_pointer) { int max_data_size; urg_connection_type_t type_eth = URG_ETHERNET; urg_measurement_type_t m_type_eth = URG_MULTIECHO; char *device_eth = "192.168.0.10"; char *device_usb = "/dev/ttyACM0"; eth = 0; usb = 0; if (urg_open(&connection_eth, type_eth, device_eth, 10940) < 0) { printf("Could not connect to the ethernet sensor\n"); } else eth = 1; connection_usb = scipConnect(device_usb); if (connection_usb == NULL) { printf("Could not connect to the usb sensor\n"); } else usb = 1; // start measurement if (eth) { max_data_size = urg_max_data_size(&connection_eth); buffer_eth = malloc(sizeof(int)*max_data_size); urg_start_measurement(&connection_eth, m_type_eth, 0, 0); last_poll_eth = utime() - poll_time_eth; } if (usb) { switchToScip2(connection_usb); scip2SetComSpeed(connection_usb,115200); last_poll_usb = utime() - poll_time_usb; } return NULL; }
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; }