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
|
/*!
\example urg_acquire.cpp
\brief Sample of acquisition sample of data
\author Satofumi KAMIMURA
$Id: urg_acquire.cpp 1683 2010-02-10 10:28:05Z satofumi $
*/
#include "findUrgPorts.h"
#include "UrgCtrl.h"
#include <cstdlib>
#include <iostream>
#include <fstream>
using namespace qrk;
using namespace std;
namespace
{
void outputCsv(ofstream& fout, const vector<long>& data)
{
for (vector<long>::const_iterator it = data.begin();
it != data.end(); ++it) {
// The distance data that are less than urg_minDistance() are shown
// as invalide values
fout << *it << ',';
}
fout << endl;
}
}
//! main
int main(int argc, char *argv[])
{
if (argc < 3) {
cerr << "usage:" << endl
<< "\t" << argv[0] << " <number of scans> <output file>\n"
<< endl;
exit(1);
}
int scan_times = atoi(argv[1]);
max(scan_times, 0);
ofstream fout(argv[2]);
if (! fout.is_open()) {
perror(argv[2]);
}
// Search URG port
vector<string> ports;
findUrgPorts(ports);
if (ports.empty()) {
cerr << "no ports." << endl;
exit(1);
}
UrgCtrl urg;
if (! urg.connect(ports[0].c_str())) {
cerr << "UrgCtrl::connect: " << urg.what() << endl;
exit(1);
}
// Get data by GD command
vector<long> data;
for (int i = 0; i < scan_times; ++i) {
int n = urg.capture(data);
if (n < 0) {
continue;
}
outputCsv(fout, data);
cout << (i + 1) << " scaned." << endl;
}
#ifdef MSC
getchar();
#endif
return 0;
}
|
