void checkAndConnect(Hok_t *hok) {
if (!hok->isWorking) {
printf("%sHokuyo not connected, trying to connect to %s\n", PREFIX, hok->path);
int error = urg_connect(hok->urg, hok->path, 115200);
if (error < 0) {
printf("%sCan't connect to hokuyo : %s\n", PREFIX, urg_error(hok->urg));
hok->isWorking = 0;
} else {
hok->imin = urg_rad2index(hok->urg, hok->cone_min);
hok->imax = urg_rad2index(hok->urg, hok->cone_max);
urg_setCaptureTimes(hok->urg, UrgInfinityTimes);
error = urg_requestData(hok->urg, URG_MD, hok->imin, hok->imax);
if (error < 0) {
printf("%sCan't connect to hokuyo\n", PREFIX);
hok->isWorking = 0;
} else {
printf("%sHokuyo connected\n", PREFIX);
hok->isWorking = 1;
printf("%sRequesting data on indexes %d to %d from %s OK\n", PREFIX, hok->imin, hok->imax, hok->path);
hok->nb_data = urg_dataMax(hok->urg);
double *angles = malloc(hok->nb_data * sizeof(double));
int i;
for (i=0; i<hok->nb_data; i++) {
angles[i] = modTwoPi(urg_index2rad(hok->urg, i) + hok->orientation);
}
hok->fm = initFastmath(hok->nb_data, angles);
free(angles);
printf("%sCalculted sin/cos data for %s\n", PREFIX, hok->path);
}
}
}
}
/*-----------------------------------------------------------*/
int read_laser_data(urg_t *urg)
{
int n;
/*If there was an error...*/
/*Getting the maximum amount of data*/
data_max = urg_dataMax(urg);
if (data_max<0)
return -1;
pthread_mutex_lock(&mutex_laser_read);
if(!data_laser)
data_laser=malloc(sizeof(long)*data_max);
//free(data);
/*Data buffer allocation (deallocation is demanded to library user)*/
/*Printing and signalling allocation error*/
if (!(data_laser)) {
fprintf(stderr, "Error in data memory allocation\n");
return -1;
}
/*Check if capturing mode is HYBRID. If it is true, check the remaining capture times,
clean the serial and request data again.*/
else
{
if(urg_capturing_mode==HYBRID)
{
/*Escamotage to avoid the laser stuck bug*/
if(urg_remainCaptureTimes(urg)<10)
{
urg_disconnect(urg);
init_urg_laser(&urg,HYBRID);
}
}
}
/*Receive the data in the buffer*/
n = urg_receiveData(urg,data_laser,data_max);
pthread_mutex_unlock(&mutex_laser_read);
/*When the on_demand mode is on (it is necessary to explicitly request data)*/
if(urg_capturing_mode==ON_DEMAND)
{
int ret = urg_requestData(urg, URG_GD, URG_FIRST, URG_LAST);
if (ret < 0) {
return ret;
}
}
/*If there was an error...*/
if (n < 0) {
return -1;
}
return n;
}
//! main
int main(int argc, char *argv[])
{
#ifdef WINDOWS_OS
const char device[] = "COM3"; /* For Windows */
#else
const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif
urg_t urg;
urg_parameter_t parameters;
int ret;
/* Connection */
urg_initialize(&urg);
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
/* Get sensor parameter */
ret = urg_parameters(&urg, ¶meters);
printf("urg_getParameters: %s\n", urg_error(&urg));
if (ret < 0) {
urg_disconnect(&urg);
exit(1);
}
/* Display */
printf("distance_min: %ld\n", parameters.distance_min_);
printf("distance_max: %ld\n", parameters.distance_max_);
printf("area_total: %d\n", parameters.area_total_);
printf("area_min: %d\n", parameters.area_min_);
printf("area_max: %d\n", parameters.area_max_);
printf("area_front: %d\n", parameters.area_front_);
printf("scan_rpm: %d\n", parameters.scan_rpm_);
printf("\n");
/* Display information from URG structure (same resource as above) */
printf("urg_getDistanceMax(): %ld\n", urg_maxDistance(&urg));
printf("urg_getDistanceMin(): %ld\n", urg_minDistance(&urg));
printf("urg_getScanMsec(): %d\n", urg_scanMsec(&urg));
printf("urg_getDataMax(): %d\n", urg_dataMax(&urg));
#ifdef MSC
getchar();
#endif
return 0;
}
//获取原始数据，这里去调用SCIP lib的库函数，填充DataFrame实体
bool CSCIPRadarComm::GetRawData(DataFrame * pData)
{
int data_max;
long *data;
int ret;
int n;
data_max = urg_dataMax(&urg);
data = new long[data_max];
if (data == NULL) {
//perror("malloc");
return false;
}
memset(data,0,sizeof(long)*data_max);
ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
if (ret < 0) {
delete data;
return false;
}
n = urg_receiveData(&urg, data, data_max);//n 表示返回的距离个数，最大1081
if (n < 0) {
delete data;
return false;
}
if( ResolveData(data,n,pData)) //解析原始数据，存入自己定义的原始数据
{
delete data;
return true;
}
else
{
delete data;
return false;
}
}
/*! main */
int main(int argc, char *argv[])
{
enum {
CaptureTimes = 10,
};
#ifdef WINDOWS_OS
const char device[] = "COM3"; /* For Windows */
#else
const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif
int data_max;
long* data;
urg_parameter_t parameter;
int ret;
int n;
int i;
/* Connection */
urg_t urg;
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
/* Request for Data using GD */
data_max = urg_dataMax(&urg);
data = (long*)malloc(sizeof(long) * data_max);
if (data == NULL) {
perror("data buffer");
exit(1);
}
urg_parameters(&urg, ¶meter);
/* Request for Data using GD */
printf("GD capture\n");
for (i = 0; i < CaptureTimes; ++i) {
/* Request for data */
ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
if (ret < 0) {
urg_exit(&urg, "urg_requestData()");
}
/* Reception */
n = urg_receiveData(&urg, data, data_max);
if (n < 0) {
urg_exit(&urg, "urg_receiveData()");
}
/* Display */
printData(&urg, ¶meter, data);
}
printf("\n");
/* Request for Data using MD */
printf("MD capture\n");
/* set data acquisition frequency equal to infinity, to get the data more
than 100 times */
/* urg_setCaptureTimes(&urg, UrgInfinityTimes); */
assert(CaptureTimes < 100);
urg_setCaptureTimes(&urg, CaptureTimes);
/* Request for data */
ret = urg_requestData(&urg, URG_MD, URG_FIRST, URG_LAST);
if (ret < 0) {
urg_exit(&urg, "urg_requestData()");
}
for (i = 0; i < CaptureTimes; ++i) {
/* Reception */
n = urg_receiveData(&urg, data, data_max);
if (n < 0) {
urg_exit(&urg, "urg_receiveData()");
}
/* Display */
printData(&urg, ¶meter, data);
}
urg_disconnect(&urg);
free(data);
#ifdef MSC
getchar();
#endif
return 0;
}
/*! main */
int main(int argc, char *argv[])
{
enum {
Times = 10,
Urgs = 2,
};
urg_t urg[Urgs];
long *data[Urgs];
int data_max[Urgs];
int timestamp;
int ret;
int n;
int i;
int k;
#ifdef WINDOWS_OS
const char *devices[] = { "COM3", "COM4" }; /* For Windows */
#else
const char *devices[] = { "/dev/ttyACM0", "/dev/ttyACM1" }; /* For Linux */
#endif
/* Connection */
for (i = 0; i < Urgs; ++i) {
urg_initialize(&urg[i]);
ret = urg_connect(&urg[i], devices[i], 115200);
if (ret < 0) {
urg_exit(&urg[i], "urg_connect()");
}
/* To clear existing MD command*/
urg_laserOff(&urg[i]);
/* It will become easy if some frames are skipped. */
/* If specified skip is 2, then transferred data becomes half. */
/* urg_setSkipLines(&urg[i], 2); */
/* Reserve for receive buffer */
data_max[i] = urg_dataMax(&urg[i]);
data[i] = (long*)malloc(sizeof(long) * data_max[i]);
if (data[i] == NULL) {
perror("data buffer");
exit(1);
}
}
/* Request for MD data */
for (i = 0; i < Urgs; ++i) {
urg_setCaptureTimes(&urg[i], Times);
/* Request for data */
ret = urg_requestData(&urg[i], URG_MD, URG_FIRST, URG_LAST);
if (ret < 0) {
urg_exit(&urg[i], "urg_requestData()");
}
}
for (k = 0; k < Times; ++k) {
for (i = 0; i < Urgs; ++i) {
/* Ends when data reception is completed */
int remain_times = urg_remainCaptureTimes(&urg[i]);
printf(" %d: ", i);
printf("(%03d/%03d): ", remain_times, Times);
if (remain_times <= 0) {
printf("\n");
continue;
}
/* Reception */
n = urg_receiveData(&urg[i], data[i], data_max[i]);
if (n < 0) {
/* Continue processing, because there is chances of receiving the
data next time. */
printf("%s: %s\n", "urg_receiveData()", urg_error(urg));
} else {
/* Display */
timestamp = urg_recentTimestamp(&urg[i]);
printf("timestamp: %d, ", timestamp);
#if 0
{
int j;
for (j = 0; j < n; ++j) {
/* Neglect if distance data is less than urg_minDistance() */
printf("%d:%ld, ", j, data[i][j]);
}
printf("\n");
}
#endif
printf("\n");
}
}
}
/* Disconnect */
for (i = 0; i < Urgs; ++i) {
urg_disconnect(&urg[i]);
free(data[i]);
}
#ifdef MSC
getchar();
#endif
return 0;
}
int main(int argc, char *argv[])
{
#ifdef WINDOWS_OS
const char device[] = "COM3"; /* For Windows */
#else
const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif
int data_max;
long *data;
int timestamp;
int ret;
int n;
int i;
/* Connection */
urg_t urg;
urg_initialize(&urg);
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
/* Reserve for reception data */
data_max = urg_dataMax(&urg);
data = (long*)malloc(sizeof(long) * data_max);
if (data == NULL) {
perror("malloc");
exit(1);
}
/* Request for GD data */
ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
if (ret < 0) {
urg_exit(&urg, "urg_requestData()");
}
/* Reception */
n = urg_receiveData(&urg, data, data_max);
printf("# n = %d\n", n);
if (n < 0) {
urg_exit(&urg, "urg_receiveData()");
}
/* Display */
timestamp = urg_recentTimestamp(&urg);
printf("# timestamp: %d\n", timestamp);
for (i = 0; i < n; ++i) {
/*Neglect the distance less than urg_minDistance() */
printf("%d %ld, ", i, data[i]);
}
printf("\n");
urg_disconnect(&urg);
free(data);
#ifdef MSC
getchar();
#endif
return 0;
}
/*! main */
int main(int argc, char *argv[])
{
#ifdef WINDOWS_OS
const char device[] = "COM3"; /* For Windows */
#else
const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif
long *data = NULL;
int data_max;
int min_length = 0;
int max_length = 0;
int ret;
int n;
int i;
/* Connection */
urg_t urg;
urg_initialize(&urg);
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
}
/* Reserve for Reception data */
data_max = urg_dataMax(&urg);
data = (long*)malloc(sizeof(long) * data_max);
if (data == NULL) {
perror("data buffer");
exit(1);
}
/* Request for GD data */
ret = urg_requestData(&urg, URG_GD, URG_FIRST, URG_LAST);
if (ret < 0) {
urg_exit(&urg, "urg_requestData()");
}
/* Reception */
n = urg_receiveData(&urg, data, data_max);
if (n < 0) {
urg_exit(&urg, "urg_receiveData()");
}
/* Output as 2 dimensional data */
/* Consider front of URG as positive direction of X axis */
min_length = urg_minDistance(&urg);
max_length = urg_maxDistance(&urg);
for (i = 0; i < n; ++i) {
int x, y;
long length = data[i];
/* Neglect the out of range values */
if ((length <= min_length) || (length >= max_length)) {
continue;
}
x = (int)(length * cos(urg_index2rad(&urg, i)));
y = (int)(length * sin(urg_index2rad(&urg, i)));
printf("%d\t%d\t# %d, %ld\n", x, y, i, length);
}
urg_disconnect(&urg);
free(data);
#ifdef MSC
getchar();
#endif
return 0;
}
//! main
int main(int argc, char *argv[])
{
enum {
CaptureTimes = 10,
};
#ifdef WINDOWS_OS
const char device[] = "COM3"; /* For Windows */
#else
const char device[] = "/dev/ttyACM0"; /* For Linux */
#endif
int data_max;
long* data;
int timestamp = -1;
int previous_timestamp;
int remain_times;
//int scan_msec;
urg_parameter_t parameter;
int ret;
int n;
int i;
urg_t urg;
/* Connection */
urg_initialize(&urg);
ret = urg_connect(&urg, device, 115200);
if (ret < 0) {
urg_exit(&urg, "urg_connect()");
exit(1);
}
/* Reserve for receive buffer */
data_max = urg_dataMax(&urg);
data = (long*)malloc(sizeof(long) * data_max);
if (data == NULL) {
fprintf(stderr, "data_max: %d\n", data_max);
perror("data buffer");
exit(1);
}
urg_parameters(&urg, ¶meter);
//scan_msec = urg_scanMsec(&urg);
/* Request for MD data */
/* To get data continuously for more than 100 times, set capture times equal
to infinity times(UrgInfinityTimes) */
/* urg_setCaptureTimes(&urg, UrgInfinityTimes); */
assert(CaptureTimes < 100);
urg_setCaptureTimes(&urg, CaptureTimes);
/* Request for data */
ret = urg_requestData(&urg, URG_MD, URG_FIRST, URG_LAST);
if (ret < 0) {
urg_exit(&urg, "urg_requestData()");
}
for (i = 0; i < CaptureTimes; ++i) {
/* Reception */
n = urg_receiveData(&urg, data, data_max);
printf("n = %d\n", n);
if (n < 0) {
urg_exit(&urg, "urg_receiveData()");
} else if (n == 0) {
printf("n == 0\n");
--i;
continue;
}
/* Display the front data with timestamp */
/* Delay in reception of data at PC causes URG to discard the data which
cannot be transmitted. This may results in remain_times to become
discontinuous */
previous_timestamp = timestamp;
timestamp = urg_recentTimestamp(&urg);
remain_times = urg_remainCaptureTimes(&urg);
/* Neglect the distance data if it is less than urg_minDistance() */
printf("%d/%d: %ld [mm], %d [msec], (%d)\n",
remain_times, CaptureTimes, data[parameter.area_front_], timestamp,
timestamp - previous_timestamp);
printf("%d, %d\n", i, remain_times);
if (remain_times <= 0) {
break;
}
}
urg_disconnect(&urg);
free(data);
#ifdef MSC
getchar();
#endif
return 0;
}
Hokuyo::SampleBuffer *Hokuyo::createSampleBuffer() {
return new SampleBuffer(urg_dataMax(&urg));
}
