static void print_data(urg_t *urg, long data[], int data_n, long time_stamp)
{
#if 1
int front_index;
(void)data_n;
front_index = urg_step2index(urg, 0);
printf("%ld [mm], (%ld [msec])\n", data[front_index], time_stamp);
#else
(void)time_stamp;
int i;
long min_distance;
long max_distance;
urg_distance_min_max(urg, &min_distance, &max_distance);
for (i = 0; i < data_n; ++i) {
long l = data[i];
double radian;
long x;
long y;
if ((l <= min_distance) || (l >= max_distance)) {
continue;
}
radian = urg_index2rad(urg, i);
x = (long)(l * cos(radian));
y = (long)(l * sin(radian));
printf("(%ld, %ld), ", x, y);
}
printf("\n");
#endif
}
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 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;
}
// 障害物を検出してその結果を配列で返す
void urg_driving::getObstacleData(float*& data_x, float*& data_y)
{
this->urg_unko::getData4URG(0, 0, 0);
data_x = new float[data_n];
data_y = new float[data_n];
int datacount = 1;
for (int i = 0; i < data_n; ++i) {
long l = data[i]; //取得した点までの距離
double radian;
float x, y, z;
float ideal_x, ideal_y;
//異常値ならとばす
if (!this->pointpos[0][i] && !this->pointpos[1][i]) continue;
//点までの角度を取得してxyに変換
//(極座標で取得されるデータをデカルト座標に変換)
radian = urg_index2rad(&urg, i);
x = (float)(l * cos(radian));
y = (float)(l * sin(radian));
z = urgpos[0];
// 右センサの領域判別
if (urgpos[2] > 0)
{
ideal_x = (float)(l * cos(radian - (double)urgpos[3]));
ideal_y = (float)(l * sin(radian - (double)urgpos[3]));
if (ideal_x < 1000.0 && ideal_y < 250.0 && ideal_y > -770.0)
//if (ideal_x < 500.0)
{
data_x[datacount] = ideal_x;
data_y[datacount] = ideal_y;
datacount++;
}
}
// 左センサの領域判別
else if (urgpos[2] < 0)
{
ideal_x = (float)(l * cos(radian - (double)urgpos[3]));
ideal_y = (float)(l * sin(radian - (double)urgpos[3]));
if (ideal_x < 1000.0 && ideal_y < 770.0 && ideal_y > -250.0)
{
data_x[datacount] = ideal_x;
data_y[datacount] = ideal_y;
datacount++;
}
}
}
data_x[0] = datacount - 1;
data_y[0] = datacount - 1;
}
// 障害物を検出してその有無を返す(過去の遺産)
urg_driving::ObstacleEmergency urg_driving::checkObstacle()
{
this->urg_unko::getData4URG(0, 0, 0);
int count = 0;
for (int i = 0; i < data_n; ++i) {
long l = data[i]; //取得した点までの距離
double radian;
float x, y, z;
float ideal_x, ideal_y;
//異常値ならとばす
if (!this->pointpos[0][i] && !this->pointpos[1][i]) continue;
//点までの角度を取得してxyに変換
//(極座標で取得されるデータをデカルト座標に変換)
radian = urg_index2rad(&urg, i);
x = (float)(l * cos(radian));
y = (float)(l * sin(radian));
z = urgpos[0];
ideal_x = (float)(l * cos(radian + (double)urgpos[3]));
ideal_y = (float)(l * sin(radian + (double)urgpos[3]));
// 右センサの領域判別
if (urgpos[2] > 0)
{
if (ideal_x < 1000.0 && ideal_y < 200.0 && ideal_y > -500.0)
//if (ideal_x < 500.0)
{
count += 1;
}
}
// 左センサの領域判別
else if (urgpos[2] < 0)
{
if (ideal_x < 1000.0 && ideal_y < 500.0 && ideal_y > -200.0)
{
count += 1;
}
}
}
if (count > 8){
return DETECT;
//printf("点の数 = %d\n", count);
}
return NONE;
}
static void print_data(urg_t *urg, long data[], int data_n, long time_stamp)
{
#if 1
int front_index;
(void)data_n;
// \~japanese 前方のデータのみを表示
front_index = urg_step2index(urg, 0);
printf("%ld [mm], (%ld [msec])\n", data[front_index], time_stamp);
#else
(void)time_stamp;
int i;
long min_distance;
long max_distance;
//open .txt file an write data into it
//FILE *fp;
//char ch;
//fp=fopen("result.txt","w");
// \~japanese 全てのデータの X-Y の位置を表示
urg_distance_min_max(urg, &min_distance, &max_distance);
for (i = 0; i < data_n; ++i) {
long l = data[i];
double radian;
long x;
long y;
if ((l <= min_distance) || (l >= max_distance)) {
continue;
}
radian = urg_index2rad(urg, i);
printf("%5.5f,%ld\n", radian/**180/3.141565*/, l+20);
//putc(radian*180/3.141565,fp);
//putc(l,fp);
//x = (long)(l * cos(radian));
//y = (long)(l * sin(radian));
//printf("(%ld, %ld), ", x, y);
}
//fclose(fp);
printf("\n");
#endif
}
int urg_index2deg(const urg_t *urg, int index)
{
int degree = (int)floor((urg_index2rad(urg, index) * 180 / M_PI) + 0.5);
return degree;
}
/*! 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;
}
double urg_index2deg(const urg_t *urg, int index)
{
return urg_index2rad(urg, index) * 180.0 / M_PI;
}
int processData(urg_t *urg, long *data, int data_n, double walling) {
double minDistance = 10000; // Closest point, starts out of range, lowers as closer points are found
int startPointL = -1; // Index of point at the left of the sensor (starts off invalid to confirm)
int startPointR = -1; // Index of point at the right of the sensor (starts off invalid to confirm)
const int min_distance = 20; // Closest accurate measurable distance of sensor
const int max_distance = 5600; // Furthest accurate measureable distance of sensor
double coordsX[700]; // Store X coordinates of points
double coordsY[700]; // Store Y coordinates of points
int coords_n = 0; // Number of points in coords[][]
int lineAssign[700] = {0}; // Array to determine whether a point has been assigned to a wall line
int nL; // Number of elements in the left wall line
double lL; // Length of the left wall line
double aL; // Y-offset of the line (distance left (positive) or right (negative) from the sensor)
double bL; // Slope of the line (0 = parallel to forward motion)
double endPointsL[2]; // Start and end x-coordinate of line (x-coordinate along computed regression line)
int nR;
double lR;
double aR;
double bR;
double endPointsR[2];
int index = 0; // Index at which to store the processed data in coords[][]
int i;
for (i = 0; i < data_n; ++i) {
long l = data[i];
double radian;
long double x;
long double y;
if ((l <= min_distance) || (l >= max_distance)) { // Do not include points out of sensor's effective range
continue;
}
radian = urg_index2rad(urg, i); // Convert to radian using the data point's index
x = (l * cos(radian)); // Convert to x, y
y = (l * sin(radian));
coordsX[index] = x; // Store x in 1st sub-array
coordsY[index] = y; // Store y in 2nd sub-array
lineAssign[index] = 0;
if ((l < minDistance) && (y < 100.0) && (y > -100.0)) { // Search for obstacles ahead in 20cm window
minDistance = l;
}
if ((x < 1.0) && (x > -1.0)) { // Look for the points directly left and right of the sensor
if (y < 0.0) startPointR = index; // Find startPoint for right line
if (y > 0.0) startPointL = index; // Find startPoint for left line
}
//printf("%d %d %lf, %lf\n", index, i, coordsX[index], coordsY[index]);
index++; // Increment index to store xy in coords[][]
}
coords_n = index; // Store final number of good points
int lostL = 1;
lostL = analyze(startPointL, coordsX, coordsY, coords_n, lineAssign, &nL, &lL, &aL, &bL, endPointsL);
if (lostL == 0) {
printf("Left: y = %lf + %lfx ", aL, bL);
printf("Length: %lf\n", lL);
}
int lostR = 1;
lostR = analyze(startPointR, coordsX, coordsY, coords_n, lineAssign, &nR, &lR, &aR, &bR, endPointsR);
if (lostR == 0) {
printf("Right: y = %lf + %lfx ", aR, bR);
printf("Length: %lf\n", lR);
}
control(lostL, lL, aL, bL, endPointsL, lostR, lR, aR, bR, endPointsR, minDistance, walling); // Straight line motor control and location update
return (lostR * 2) + lostL;
}
