Ejemplo n.º 1
0
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);

}
Ejemplo n.º 2
0
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, &timestamp);
    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;
}
Ejemplo n.º 3
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;
}
Ejemplo n.º 4
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;
}
Ejemplo n.º 5
0
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, &timestamp);
    // \todo process length_data array

    // \~japanese 取得したタイムスタンプを出力する
    // \~english Outputs the received timestamp value
    printf("%ld\n", timestamp);
}
return 0;
}
Ejemplo n.º 6
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;
}
Ejemplo n.º 7
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;
}
Ejemplo n.º 8
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;
}
Ejemplo n.º 9
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;
}
Ejemplo n.º 10
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;
}
Ejemplo n.º 11
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;
}
Ejemplo n.º 12
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,&timestamp);
	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;
}
Ejemplo n.º 13
0
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,&timestamp);
	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;
}
Ejemplo n.º 14
0
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;
}
Ejemplo n.º 15
0
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;
}