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