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[])
{
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 = 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;
}
