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; }
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; }