1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
|
/*!
\example captureIntensitySample.cpp
\brief Sample to get intensity data
\author Satofumi KAMIMURA
$Id: captureIntensitySample.cpp 1684 2010-02-10 23:56:38Z satofumi $
*/
#include "UrgCtrl.h"
#include "TcpipSocket.h"
#include "delay.h"
#include <SDL.h>
#include <cstdlib>
using namespace qrk;
using namespace std;
int main(int argc, char *argv[])
{
const char address[] = "192.168.0.10";
short port = 10940;
TcpipSocket tcpip;
if (! tcpip.connect(address, port)) {
printf("TcpipSocket::connect: %s\n", tcpip.what());
exit(1);
}
UrgCtrl urg;
urg.setConnection(&tcpip);
urg.loadParameter();
int scan_msec = urg.scanMsec();
urg.setCaptureMode(IntensityCapture);
urg.setCaptureRange(0, 760);
enum {
CaptureTimes = 10,
};
vector<long> data;
vector<long> intensity_data;
long previous_timestamp = 0;
for (int i = 0; i < CaptureTimes; ++i) {
fprintf(stderr, ".");
long timestamp = 0;
int data_n = urg.captureWithIntensity(data, intensity_data, ×tamp);
if (data_n > 0) {
int front_index = urg.rad2index(0.0);
#if 1
printf("%d: %ld [mm] (%ld), %ld [msec] (%ld)\n",
i, data[front_index], intensity_data[front_index], timestamp,
timestamp - previous_timestamp);
#else
(void)front_index;
for (int j = 0; j < data_n; ++j) {
printf("%ld(%ld), ", data[j], intensity_data[j]);
}
printf("\n");
#endif
previous_timestamp = timestamp;
} else {
--i;
delay(scan_msec);
}
}
#ifdef MSC
getchar();
#endif
return 0;
}
|
