Apollo 10.0
自动驾驶开放平台
velodyne_parser.h
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
17/* -*- mode: C++ -*-
18 *
19 * Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson
20 * Copyright (C) 2009 Austin Robot Technology, Jack O'Quin
21 *
22 * License: Modified BSD Software License Agreement
23 *
24 * $Id$
25 */
26
51#pragma once
52
53#include <cerrno>
54#include <cmath>
55#include <cstdint>
56#include <limits>
57#include <memory>
58#include <string>
59
60#include <boost/format.hpp>
61
62#include "modules/drivers/lidar/proto/velodyne.pb.h"
63#include "modules/drivers/lidar/proto/velodyne_config.pb.h"
64#include "modules/common_msgs/sensor_msgs/pointcloud.pb.h"
65
69
70namespace apollo {
71namespace drivers {
72namespace velodyne {
73
88
92static const int BLOCK_SIZE = 100;
93static const int RAW_SCAN_SIZE = 3;
94static const int SCANS_PER_BLOCK = 32;
95static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);
96
97static const float ROTATION_RESOLUTION = 0.01f;
98// static const uint16_t ROTATION_MAX_UNITS = 36000; [>*< hundredths of degrees
99// <]
100// because angle_rang is [0, 36000], so the size is 36001
101static const uint16_t ROTATION_MAX_UNITS = 36001;
105static const float DISTANCE_MAX = 130.0f;
106static const float DISTANCE_RESOLUTION = 0.002f;
107static const float DISTANCE_MAX_UNITS =
108 (DISTANCE_MAX / DISTANCE_RESOLUTION + 1.0f);
109
110// laser_block_id
111static const uint16_t UPPER_BANK = 0xeeff;
112static const uint16_t LOWER_BANK = 0xddff;
113
114static const float ANGULAR_RESOLUTION = 0.00300919f;
115
117static const int VLP16_FIRINGS_PER_BLOCK = 2;
118static const int VLP16_SCANS_PER_FIRING = 16;
119static const float VLP16_BLOCK_TDURATION = 110.592f;
120static const float VLP16_DSR_TOFFSET = 2.304f;
121static const float VLP16_FIRING_TOFFSET = 55.296f;
122
123static const float VLS128_CHANNEL_TDURATION = 2.304f;
124static const float VLS128_SEQ_TDURATION = 55.296f;
125
126static const float VLP32_DISTANCE_RESOLUTION = 0.004f;
127static const float VSL128_DISTANCE_RESOLUTION = 0.004f;
128static const float CHANNEL_TDURATION = 2.304f;
129static const float SEQ_TDURATION = 55.296f;
130
138struct RawBlock {
139 uint16_t laser_block_id;
140 uint16_t rotation;
141 uint8_t data[BLOCK_DATA_SIZE];
142};
143
150 uint16_t raw_distance;
151 uint8_t bytes[2];
152};
153
154static const int PACKET_SIZE = 1206;
155static const int BLOCKS_PER_PACKET = 12;
156static const int PACKET_STATUS_SIZE = 4;
157static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
158
160 HOURS = 72,
163 DATE = 68,
164 MONTH = 78,
165 YEAR = 89,
166 GPS_STATUS = 71
168
178struct RawPacket {
180 // uint16_t revolution;
181 // uint8_t status[PACKET_STATUS_SIZE];
182 unsigned int gps_timestamp;
183 unsigned char status_type;
184 unsigned char status_value;
185};
186
188// static const int VLS128_NUM_CHANS_PER_BLOCK = 128;
189// static const int VLS128_BLOCK_DATA_SIZE = VLS128_NUM_CHANS_PER_BLOCK *
190// RAW_SCAN_SIZE;
191//
192// struct Vls128RawBlock {
193// uint16_t header;
194// uint16_t rotation;
195// uint8_t data[VLS128_BLOCK_DATA_SIZE];
196// };
197//
198// struct Vls128RawPacket {
199// Vls128RawBlock blocks[3];
200// uint16_t revolution;
201// uint8_t status[PACKET_STATUS_SIZE];
202// };
203
204// Convert related config, get value from private_nh param server, used by
205// velodyne raw data
206// struct Config {
207// double max_range; ///< maximum range to publish
208// double min_range; ///< minimum range to publish
209// double max_angle;
210// double min_angle;
211// double view_direction;
212// double view_width;
213// bool calibration_online;
214// std::string calibration_file;
215// std::string model; // VLP16,32E, 64E_32
216// bool organized; // is point cloud Order
217//};
218
219// enum Mode { STRONGEST, LAST, DUAL };
220
221static const float nan = std::numeric_limits<float>::signaling_NaN();
222
225 public:
227 explicit VelodyneParser(const Config& config);
228 virtual ~VelodyneParser() {}
229
241 virtual void GeneratePointcloud(const std::shared_ptr<VelodyneScan>& scan_msg,
242 std::shared_ptr<PointCloud> out_msg) = 0;
243 virtual void setup();
244 // Order point cloud fod IDL by velodyne model
245 virtual void Order(std::shared_ptr<PointCloud> cloud) = 0;
246
248 const double get_last_timestamp() { return last_time_stamp_; }
249
250 protected:
251 const float (*inner_time_)[12][32];
252
254 float sin_rot_table_[ROTATION_MAX_UNITS];
255 float cos_rot_table_[ROTATION_MAX_UNITS];
258 // Last Velodyne packet time stamp. (Full time)
261
262 PointXYZIT get_nan_point(uint64_t timestamp);
263 void init_angle_params(double view_direction, double view_width);
270 void ComputeCoords(const float& raw_distance,
271 const LaserCorrection& corrections,
272 const uint16_t rotation, PointXYZIT* point);
273
274 bool is_scan_valid(int rotation, float distance);
275
280 virtual void Unpack(const VelodynePacket& pkt,
281 std::shared_ptr<PointCloud> pc) = 0;
282
283 uint64_t GetGpsStamp(double current_stamp, double* previous_stamp,
284 uint64_t* gps_base_usec);
285
286 virtual uint64_t GetTimestamp(double base_time, float time_offset,
287 uint16_t laser_block_id) = 0;
288}; // class VelodyneParser
289
291 public:
292 explicit Velodyne64Parser(const Config& config);
294
295 void GeneratePointcloud(const std::shared_ptr<VelodyneScan>& scan_msg,
296 std::shared_ptr<PointCloud> out_msg);
297 void Order(std::shared_ptr<PointCloud> cloud);
298 void setup() override;
299
300 private:
301 void SetBaseTimeFromPackets(const VelodynePacket& pkt);
302 void CheckGpsStatus(const VelodynePacket& pkt);
303 uint64_t GetTimestamp(double base_time, float time_offset,
304 uint16_t laser_block_id);
305 void Unpack(const VelodynePacket& pkt, std::shared_ptr<PointCloud> pc);
306 void InitOffsets();
307 int IntensityCompensate(const LaserCorrection& corrections,
308 const uint16_t raw_distance, int intensity);
309 // Previous Velodyne packet time stamp. (offset to the top hour)
310 double previous_packet_stamp_[4];
311 uint64_t gps_base_usec_[4]; // full time
312 bool is_s2_;
313 int offsets_[64];
314
315 OnlineCalibration online_calibration_;
316}; // class Velodyne64Parser
317
319 public:
320 explicit Velodyne32Parser(const Config& config);
322
323 void GeneratePointcloud(const std::shared_ptr<VelodyneScan>& scan_msg,
324 std::shared_ptr<PointCloud> out_msg);
325 void Order(std::shared_ptr<PointCloud> cloud);
326
327 private:
328 uint64_t GetTimestamp(double base_time, float time_offset,
329 uint16_t laser_block_id);
330 void Unpack(const VelodynePacket& pkt, std::shared_ptr<PointCloud> pc);
331 void UnpackVLP32C(const VelodynePacket& pkt, std::shared_ptr<PointCloud> pc);
332 // Previous laser firing time stamp. (offset to the top hour)
333 double previous_firing_stamp_;
334 uint64_t gps_base_usec_; // full time
335}; // class Velodyne32Parser
336
338 public:
339 explicit Velodyne16Parser(const Config& config);
341
342 void GeneratePointcloud(const std::shared_ptr<VelodyneScan>& scan_msg,
343 std::shared_ptr<PointCloud> out_msg);
344 void Order(std::shared_ptr<PointCloud> cloud);
345
346 private:
347 uint64_t GetTimestamp(double base_time, float time_offset,
348 uint16_t laser_block_id);
349 void Unpack(const VelodynePacket& pkt, std::shared_ptr<PointCloud> pc);
350 // Previous Velodyne packet time stamp. (offset to the top hour)
351 double previous_packet_stamp_;
352 uint64_t gps_base_usec_; // full time
353}; // class Velodyne16Parser
354
356 public:
357 explicit Velodyne128Parser(const Config& config);
359
360 void GeneratePointcloud(const std::shared_ptr<VelodyneScan>& scan_msg,
361 std::shared_ptr<PointCloud> out_msg);
362 void Order(std::shared_ptr<PointCloud> cloud);
363
364 private:
365 uint64_t GetTimestamp(double base_time, float time_offset,
366 uint16_t laser_block_id);
367 void Unpack(const VelodynePacket& pkt, std::shared_ptr<PointCloud> pc);
368 int IntensityCompensate(const LaserCorrection& corrections,
369 const uint16_t raw_distance, int intensity);
370 // Previous Velodyne packet time stamp. (offset to the top hour)
371 double previous_packet_stamp_;
372 uint64_t gps_base_usec_; // full time
373}; // class Velodyne128Parser
374
376 public:
377 static VelodyneParser* CreateParser(Config config);
378};
379
380} // namespace velodyne
381} // namespace drivers
382} // namespace apollo
Calibration class storing entire configuration for the Velodyne
Definition calibration.h:68
void GeneratePointcloud(const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)
Set up for data processing.
void Order(std::shared_ptr< PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)
Set up for data processing.
void Order(std::shared_ptr< PointCloud > cloud)
void Order(std::shared_ptr< PointCloud > cloud)
void GeneratePointcloud(const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)
Set up for data processing.
void GeneratePointcloud(const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)
Set up for data processing.
void setup() override
Set up for on-line operation.
void Order(std::shared_ptr< PointCloud > cloud)
static VelodyneParser * CreateParser(Config config)
virtual void setup()
Set up for on-line operation.
virtual void GeneratePointcloud(const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)=0
Set up for data processing.
bool is_scan_valid(int rotation, float distance)
uint64_t GetGpsStamp(double current_stamp, double *previous_stamp, uint64_t *gps_base_usec)
PointXYZIT get_nan_point(uint64_t timestamp)
void init_angle_params(double view_direction, double view_width)
virtual uint64_t GetTimestamp(double base_time, float time_offset, uint16_t laser_block_id)=0
virtual void Unpack(const VelodynePacket &pkt, std::shared_ptr< PointCloud > pc)=0
Unpack velodyne packet
void ComputeCoords(const float &raw_distance, const LaserCorrection &corrections, const uint16_t rotation, PointXYZIT *point)
Compute coords with the data in block
virtual void Order(std::shared_ptr< PointCloud > cloud)=0
constexpr int BLOCK_SIZE
Raw Velodyne packet constants and structures.
Definition driver.h:34
constexpr int BLOCKS_PER_PACKET
Definition driver.h:33
class register implement
Definition arena_queue.h:37
correction values for a single laser
Definition calibration.h:43
uint16_t rotation
0-35999, divide by 100 to get degrees
uint16_t laser_block_id
UPPER_BANK or LOWER_BANK
RawBlock blocks[BLOCKS_PER_PACKET]
used for unpacking the first two data bytes in a block