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
79
80
81
82
83
84
85
86
87
88
89
90
91
92
|
/*!
\example mdCaptureSample.cpp
\brief Sample to get data using MD command
\author Satofumi KAMIMURA
$Id: mdCaptureSample.cpp 1683 2010-02-10 10:28:05Z satofumi $
*/
#include "UrgCtrl.h"
#include "delay.h"
#include "ticks.h"
#include <cstdlib>
#include <cstdio>
using namespace qrk;
using namespace std;
//! main
int main(int argc, char *argv[])
{
#ifdef WINDOWS_OS
const char device[] = "COM3";
#else
const char device[] = "/dev/ttyACM0";
#endif
UrgCtrl urg;
if (! urg.connect(device)) {
printf("UrgCtrl::connect: %s\n", urg.what());
exit(1);
}
#if 1
// Set to MD mode to acquire data
urg.setCaptureMode(AutoCapture);
#else
// Mode to get distance data and intensity data
urg.setCaptureMode(IntensityCapture);
urg.setCaptureSkipLines(2);
#endif
int scan_msec = urg.scanMsec();
#if 0
// Set range of acquisition from the center to left 90 degree.
// Set range of acquistion from center to right to 90 degree.
// So In total it will be 180 degree.
const double rad90 = 90.0 * M_PI / 180.0;
urg.setCaptureRange(urg.rad2index(-rad90), urg.rad2index(rad90));
#endif
int pre_timestamp = ticks();
// Data is acquired continuously using MD command
// but outputs data of specified number of times.
enum { CaptureTimes = 10};
urg.setCaptureTimes(CaptureTimes);
for (int i = 0; i < CaptureTimes;) {
long timestamp = 0;
vector<long> data;
// Get data
int n = urg.capture(data, ×tamp);
if (n <= 0) {
delay(scan_msec);
continue;
}
// Display
printf("timestamp: %ld, (%d), %ld\n",
timestamp, ticks(), timestamp - pre_timestamp);
pre_timestamp = timestamp;
#if 0
for (int j = 0; j < n; ++j) {
// The distance data that are less than urg_minDistance() are shown
// as invalide values
printf("%d:%ld, ", j, data[j]);
}
printf("\n");
#endif
++i;
}
#ifdef MSC
getchar();
#endif
return 0;
}
|
