forked from ouster-lidar/ouster-sdk
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathclient_example.cpp
More file actions
197 lines (163 loc) · 7.21 KB
/
Copy pathclient_example.cpp
File metadata and controls
197 lines (163 loc) · 7.21 KB
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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
/**
* Copyright (c) 2022, Ouster, Inc.
* All rights reserved.
*/
#include <chrono>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <vector>
#include "ouster/client.h"
#include "ouster/impl/build.h"
#include "ouster/lidar_scan.h"
#include "ouster/types.h"
using namespace ouster;
const size_t N_SCANS = 5;
const size_t UDP_BUF_SIZE = 65536;
void FATAL(const char* msg) {
std::cerr << msg << std::endl;
std::exit(EXIT_FAILURE);
}
int main(int argc, char* argv[]) {
if (argc != 2 && argc != 3) {
std::cerr
<< "Version: " << ouster::SDK_VERSION_FULL << " ("
<< ouster::BUILD_SYSTEM << ")"
<< "\n\nUsage: client_example <sensor_hostname> [<udp_destination>]"
"\n\n<udp_destination> is optional: leave blank for "
"automatic destination detection"
<< std::endl;
return argc == 1 ? EXIT_SUCCESS : EXIT_FAILURE;
}
std::cerr << "Ouster client example " << ouster::SDK_VERSION << std::endl;
/*
* The sensor client consists of the network client and a library for
* reading and working with data.
*
* The network client supports reading and writing a limited number of
* configuration parameters and receiving data without working directly with
* the socket APIs. See the `client.h` for more details. The minimum
* required parameters are the sensor hostname/ip and the data destination
* hostname/ip.
*/
const std::string sensor_hostname = argv[1];
const std::string data_destination = (argc == 3) ? argv[2] : "";
std::cerr << "Connecting to \"" << sensor_hostname << "\"... ";
auto handle = sensor::init_client(sensor_hostname, data_destination);
if (!handle) FATAL("Failed to connect");
std::cerr << "ok" << std::endl;
/*
* Configuration and calibration parameters can be queried directly from the
* sensor. These are required for parsing the packet stream and calculating
* accurate point clouds.
*/
std::cerr << "Gathering metadata..." << std::endl;
auto metadata = sensor::get_metadata(*handle);
// Raw metadata can be parsed into a `sensor_info` struct
sensor::sensor_info info = sensor::parse_metadata(metadata);
size_t w = info.format.columns_per_frame;
size_t h = info.format.pixels_per_column;
ouster::sensor::ColumnWindow column_window = info.format.column_window;
std::cerr << " Firmware version: " << info.fw_rev
<< "\n Serial number: " << info.sn
<< "\n Product line: " << info.prod_line
<< "\n Scan dimensions: " << w << " x " << h
<< "\n Column window: [" << column_window.first << ", "
<< column_window.second << "]" << std::endl;
// A LidarScan holds lidar data for an entire rotation of the device
std::vector<LidarScan> scans{
N_SCANS, LidarScan{w, h, info.format.udp_profile_lidar}};
// A ScanBatcher can be used to batch packets into scans
sensor::packet_format pf = sensor::get_format(info);
ScanBatcher batch_to_scan(info.format.columns_per_frame, pf);
/*
* The network client provides some convenience wrappers around socket APIs
* to facilitate reading lidar and IMU data from the network. It is also
* possible to configure the sensor offline and read data directly from a
* UDP socket.
*/
std::cerr << "Capturing points... ";
// buffer to store raw packet data
auto packet_buf = std::make_unique<uint8_t[]>(UDP_BUF_SIZE);
for (size_t i = 0; i < N_SCANS;) {
// wait until sensor data is available
sensor::client_state st = sensor::poll_client(*handle);
// check for error status
if (st & sensor::CLIENT_ERROR)
FATAL("Sensor client returned error state!");
// check for lidar data, read a packet and add it to the current batch
if (st & sensor::LIDAR_DATA) {
if (!sensor::read_lidar_packet(*handle, packet_buf.get(), pf)) {
FATAL("Failed to read a packet of the expected size!");
}
// batcher will return "true" when the current scan is complete
if (batch_to_scan(packet_buf.get(), scans[i])) {
// retry until we receive a full set of valid measurements
// (accounting for azimuth_window settings if any)
if (scans[i].complete(info.format.column_window)) i++;
}
}
// check if IMU data is available (but don't do anything with it)
if (st & sensor::IMU_DATA) {
sensor::read_imu_packet(*handle, packet_buf.get(), pf);
}
}
std::cerr << "ok" << std::endl;
/*
* The example code includes functions for efficiently and accurately
* computing point clouds from range measurements. LidarScan data can
* also be accessed directly using the Eigen[0] linear algebra library.
*
* [0] http://eigen.tuxfamily.org
*/
std::cerr << "Computing point clouds... " << std::endl;
// pre-compute a table for efficiently calculating point clouds from
// range
XYZLut lut = ouster::make_xyz_lut(info);
std::vector<LidarScan::Points> clouds;
for (const LidarScan& scan : scans) {
// compute a point cloud using the lookup table
clouds.push_back(ouster::cartesian(scan, lut));
// channel fields can be queried as well
auto n_valid_first_returns = (scan.field(sensor::RANGE) != 0).count();
// LidarScan also provides access to header information such as
// status and timestamp
auto status = scan.status();
auto it = std::find_if(status.data(), status.data() + status.size(),
[](const uint32_t s) {
return (s & 0x01);
}); // find first valid status
if (it != status.data() + status.size()) {
auto ts_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::nanoseconds(scan.timestamp()(
it - status.data()))); // get corresponding timestamp
std::cerr << " Frame no. " << scan.frame_id << " with "
<< n_valid_first_returns << " valid first returns at "
<< ts_ms.count() << " ms" << std::endl;
}
}
/*
* Write output to CSV files. The output can be viewed in a point cloud
* viewer like CloudCompare:
*
* [0] https://github.com/cloudcompare/cloudcompare
*/
std::cerr << "Writing files... " << std::endl;
int file_ind = 0;
std::string file_base{"cloud_"};
for (const LidarScan::Points& cloud : clouds) {
std::string filename = file_base + std::to_string(file_ind++) + ".csv";
std::ofstream out;
out.open(filename);
out << std::fixed << std::setprecision(4);
// write each point, filtering out points without returns
for (int i = 0; i < cloud.rows(); i++) {
auto xyz = cloud.row(i);
if (!xyz.isApproxToConstant(0.0))
out << xyz(0) << ", " << xyz(1) << ", " << xyz(2) << std::endl;
}
out.close();
std::cerr << " Wrote " << filename << std::endl;
}
return EXIT_SUCCESS;
}