Apollo 10.0
自动驾驶开放平台
apollo::drivers::velodyne::VelodyneParser类 参考abstract

Velodyne data conversion class 更多...

#include <velodyne_parser.h>

类 apollo::drivers::velodyne::VelodyneParser 继承关系图:
apollo::drivers::velodyne::VelodyneParser 的协作图:

Public 成员函数

 VelodyneParser ()
 
 VelodyneParser (const Config &config)
 
virtual ~VelodyneParser ()
 
virtual void GeneratePointcloud (const std::shared_ptr< VelodyneScan > &scan_msg, std::shared_ptr< PointCloud > out_msg)=0
 Set up for data processing.
 
virtual void setup ()
 Set up for on-line operation.
 
virtual void Order (std::shared_ptr< PointCloud > cloud)=0
 
const Calibrationget_calibration ()
 
const double get_last_timestamp ()
 

Protected 成员函数

PointXYZIT get_nan_point (uint64_t timestamp)
 
void init_angle_params (double view_direction, double view_width)
 
void ComputeCoords (const float &raw_distance, const LaserCorrection &corrections, const uint16_t rotation, PointXYZIT *point)
 Compute coords with the data in block
 
bool is_scan_valid (int rotation, float distance)
 
virtual void Unpack (const VelodynePacket &pkt, std::shared_ptr< PointCloud > pc)=0
 Unpack velodyne packet
 
uint64_t GetGpsStamp (double current_stamp, double *previous_stamp, uint64_t *gps_base_usec)
 
virtual uint64_t GetTimestamp (double base_time, float time_offset, uint16_t laser_block_id)=0
 

Protected 属性

const float(* inner_time_ )[12][32]
 
Calibration calibration_
 
float sin_rot_table_ [ROTATION_MAX_UNITS]
 
float cos_rot_table_ [ROTATION_MAX_UNITS]
 
double last_time_stamp_
 
Config config_
 
bool need_two_pt_correction_
 
Mode mode_
 

详细描述

Velodyne data conversion class

在文件 velodyne_parser.h224 行定义.

构造及析构函数说明

◆ VelodyneParser() [1/2]

apollo::drivers::velodyne::VelodyneParser::VelodyneParser ( )
inline

在文件 velodyne_parser.h226 行定义.

226{}

◆ VelodyneParser() [2/2]

apollo::drivers::velodyne::VelodyneParser::VelodyneParser ( const Config config)
explicit

◆ ~VelodyneParser()

virtual apollo::drivers::velodyne::VelodyneParser::~VelodyneParser ( )
inlinevirtual

在文件 velodyne_parser.h228 行定义.

228{}

成员函数说明

◆ ComputeCoords()

void apollo::drivers::velodyne::VelodyneParser::ComputeCoords ( const float &  raw_distance,
const LaserCorrection corrections,
const uint16_t  rotation,
PointXYZIT point 
)
protected

Compute coords with the data in block

参数
tmpA two bytes union store the value of laser distance information
indexThe index of block

Use standard ROS coordinate system (right-hand rule)

在文件 velodyne_parser.cc126 行定义.

128 {
129 // ROS_ASSERT_MSG(rotation < 36000, "rotation must between 0 and 35999");
130 assert(rotation <= 36000);
131 double x = 0.0;
132 double y = 0.0;
133 double z = 0.0;
134
135 double distance = raw_distance + corrections.dist_correction;
136
137 // cos(a-b) = cos(a)*cos(b) + sin(a)*sin(b)
138 // sin(a-b) = sin(a)*cos(b) - cos(a)*sin(b)
139 double cos_rot_angle =
140 cos_rot_table_[rotation] * corrections.cos_rot_correction +
141 sin_rot_table_[rotation] * corrections.sin_rot_correction;
142 double sin_rot_angle =
143 sin_rot_table_[rotation] * corrections.cos_rot_correction -
144 cos_rot_table_[rotation] * corrections.sin_rot_correction;
145
146 // double vert_offset = corrections.vert_offset_correction;
147
148 // Compute the distance in the xy plane (w/o accounting for rotation)
149 double xy_distance = distance * corrections.cos_vert_correction;
150
151 // Calculate temporal X, use absolute value.
152 double xx = fabs(xy_distance * sin_rot_angle -
153 corrections.horiz_offset_correction * cos_rot_angle);
154 // Calculate temporal Y, use absolute value
155 double yy = fabs(xy_distance * cos_rot_angle +
156 corrections.horiz_offset_correction * sin_rot_angle);
157
158 // Get 2points calibration values,Linear interpolation to get distance
159 // correction for X and Y, that means distance correction use
160 // different value at different distance
161 double distance_corr_x = 0;
162 double distance_corr_y = 0;
163
164 if (need_two_pt_correction_ && raw_distance <= 2500) {
165 distance_corr_x =
166 (corrections.dist_correction - corrections.dist_correction_x) *
167 (xx - 2.4) / 22.64 +
168 corrections.dist_correction_x; // 22.64 = 25.04 - 2.4
169 distance_corr_y =
170 (corrections.dist_correction - corrections.dist_correction_y) *
171 (yy - 1.93) / 23.11 +
172 corrections.dist_correction_y; // 23.11 = 25.04 - 1.93
173 } else {
174 distance_corr_x = distance_corr_y = corrections.dist_correction;
175 }
176
177 double distance_x = raw_distance + distance_corr_x;
178
179 xy_distance = distance_x * corrections.cos_vert_correction;
180 // xy_distance = distance_x * cos_vert_correction - vert_offset *
181 // sin_vert_correction;
182
183 x = xy_distance * sin_rot_angle -
184 corrections.horiz_offset_correction * cos_rot_angle;
185
186 double distance_y = raw_distance + distance_corr_y;
187 xy_distance = distance_y * corrections.cos_vert_correction;
188 // xy_distance = distance_y * cos_vert_correction - vert_offset *
189 // sin_vert_correction;
190 y = xy_distance * cos_rot_angle +
191 corrections.horiz_offset_correction * sin_rot_angle;
192
193 z = distance * corrections.sin_vert_correction +
194 corrections.vert_offset_correction;
195 // z = distance * sin_vert_correction + vert_offset * cos_vert_correction;
196
198 point->set_x(static_cast<float>(y));
199 point->set_y(static_cast<float>(-x));
200 point->set_z(static_cast<float>(z));
201}

◆ GeneratePointcloud()

virtual void apollo::drivers::velodyne::VelodyneParser::GeneratePointcloud ( const std::shared_ptr< VelodyneScan > &  scan_msg,
std::shared_ptr< PointCloud out_msg 
)
pure virtual

Set up for data processing.

Perform initializations needed before data processing can begin:

  • read device-specific angles calibration
参数
private_nhprivate node handle for ROS parameters
返回
0 if successful; errno value for failure

apollo::drivers::velodyne::Velodyne64Parser, apollo::drivers::velodyne::Velodyne32Parser, apollo::drivers::velodyne::Velodyne16Parser , 以及 apollo::drivers::velodyne::Velodyne128Parser 内被实现.

◆ get_calibration()

const Calibration & apollo::drivers::velodyne::VelodyneParser::get_calibration ( )
inline

在文件 velodyne_parser.h247 行定义.

◆ get_last_timestamp()

const double apollo::drivers::velodyne::VelodyneParser::get_last_timestamp ( )
inline

在文件 velodyne_parser.h248 行定义.

248{ return last_time_stamp_; }

◆ get_nan_point()

PointXYZIT apollo::drivers::velodyne::VelodyneParser::get_nan_point ( uint64_t  timestamp)
protected

在文件 velodyne_parser.cc57 行定义.

57 {
58 PointXYZIT nan_point;
59 nan_point.set_timestamp(timestamp);
60 nan_point.set_x(nan);
61 nan_point.set_y(nan);
62 nan_point.set_z(nan);
63 nan_point.set_intensity(0);
64 return nan_point;
65}

◆ GetGpsStamp()

uint64_t apollo::drivers::velodyne::VelodyneParser::GetGpsStamp ( double  current_stamp,
double *  previous_stamp,
uint64_t *  gps_base_usec 
)
protected

在文件 velodyne_parser.cc26 行定义.

28 {
29 if (current_packet_stamp < *previous_packet_stamp) {
30 // plus 3600 when large jump back, discard little jump back for wrong time
31 // in lidar
32 if (std::abs(*previous_packet_stamp - current_packet_stamp) > 3599000000) {
33 *gps_base_usec += static_cast<uint64_t>(3600 * 1e6);
34 AINFO << "Base time plus 3600s. Model: " << config_.model() << std::fixed
35 << ". current:" << current_packet_stamp
36 << ", last time:" << *previous_packet_stamp;
37 } else {
38 AWARN << "Current stamp:" << std::fixed << current_packet_stamp
39 << " less than previous stamp:" << *previous_packet_stamp
40 << ". GPS time stamp maybe incorrect!";
41 }
42 } else if (*previous_packet_stamp != 0 &&
43 current_packet_stamp - *previous_packet_stamp > 100000) { // 100ms
44 AERROR << "Current stamp:" << std::fixed << current_packet_stamp
45 << " ahead previous stamp:" << *previous_packet_stamp
46 << " over 100ms. GPS time stamp incorrect!";
47 }
48
49 *previous_packet_stamp = current_packet_stamp;
50 uint64_t gps_stamp =
51 *gps_base_usec + static_cast<uint64_t>(current_packet_stamp);
52
53 gps_stamp = gps_stamp * 1000;
54 return gps_stamp;
55}
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42
#define AWARN
Definition log.h:43

◆ GetTimestamp()

virtual uint64_t apollo::drivers::velodyne::VelodyneParser::GetTimestamp ( double  base_time,
float  time_offset,
uint16_t  laser_block_id 
)
protectedpure virtual

◆ init_angle_params()

void apollo::drivers::velodyne::VelodyneParser::init_angle_params ( double  view_direction,
double  view_width 
)
protected

在文件 velodyne_parser.cc70 行定义.

71 {
72 // converting angle parameters into the velodyne reference (rad)
73 double tmp_min_angle = view_direction + view_width / 2;
74 double tmp_max_angle = view_direction - view_width / 2;
75
76 // computing positive modulo to keep theses angles into [0;2*M_PI]
77 tmp_min_angle = fmod(fmod(tmp_min_angle, 2 * M_PI) + 2 * M_PI, 2 * M_PI);
78 tmp_max_angle = fmod(fmod(tmp_max_angle, 2 * M_PI) + 2 * M_PI, 2 * M_PI);
79
80 // converting into the hardware velodyne ref (negative yaml and degrees)
81 // adding 0.5 performs a centered double to int conversion
82 config_.set_min_angle(100 * (2 * M_PI - tmp_min_angle) * 180 / M_PI + 0.5);
83 config_.set_max_angle(100 * (2 * M_PI - tmp_max_angle) * 180 / M_PI + 0.5);
85 // avoid returning empty cloud if min_angle = max_angle
86 config_.set_min_angle(0);
87 config_.set_max_angle(36000);
88 }
89}

◆ is_scan_valid()

bool apollo::drivers::velodyne::VelodyneParser::is_scan_valid ( int  rotation,
float  distance 
)
protected

在文件 velodyne_parser.cc108 行定义.

108 {
109 // check range first
110 if (range < config_.min_range() || range > config_.max_range()) {
111 return false;
112 }
113 // condition added to avoid calculating points which are not
114 // in the interesting defined area (min_angle < area < max_angle)
115 // not used now
116 // if ((config_.min_angle > config_.max_angle && (rotation <=
117 // config_.max_angle || rotation >= config_.min_angle))
118 // || (config_.min_angle < config_.max_angle && rotation >=
119 // config_.min_angle
120 // && rotation <= config_.max_angle)) {
121 // return true;
122 // }
123 return true;
124}

◆ Order()

virtual void apollo::drivers::velodyne::VelodyneParser::Order ( std::shared_ptr< PointCloud cloud)
pure virtual

◆ setup()

void apollo::drivers::velodyne::VelodyneParser::setup ( )
virtual

Set up for on-line operation.

apollo::drivers::velodyne::Velodyne64Parser 重载.

在文件 velodyne_parser.cc92 行定义.

92 {
95
97 AFATAL << "Unable to open calibration file: "
99 }
100 }
101
102 // setup angle parameters.
105 ROTATION_RESOLUTION);
106}
void read(const std::string &calibration_file)
void init_angle_params(double view_direction, double view_width)
#define AFATAL
Definition log.h:45
void init_sin_cos_rot_table(float *sin_rot_table, float *cos_rot_table, uint16_t rotation, float rotation_resolution)
Definition util.cc:23

◆ Unpack()

virtual void apollo::drivers::velodyne::VelodyneParser::Unpack ( const VelodynePacket pkt,
std::shared_ptr< PointCloud pc 
)
protectedpure virtual

Unpack velodyne packet

类成员变量说明

◆ calibration_

Calibration apollo::drivers::velodyne::VelodyneParser::calibration_
protected

在文件 velodyne_parser.h253 行定义.

◆ config_

Config apollo::drivers::velodyne::VelodyneParser::config_
protected

在文件 velodyne_parser.h257 行定义.

◆ cos_rot_table_

float apollo::drivers::velodyne::VelodyneParser::cos_rot_table_[ROTATION_MAX_UNITS]
protected

在文件 velodyne_parser.h255 行定义.

◆ inner_time_

const float(* apollo::drivers::velodyne::VelodyneParser::inner_time_)[12][32]
protected

在文件 velodyne_parser.h251 行定义.

◆ last_time_stamp_

double apollo::drivers::velodyne::VelodyneParser::last_time_stamp_
protected

在文件 velodyne_parser.h256 行定义.

◆ mode_

Mode apollo::drivers::velodyne::VelodyneParser::mode_
protected

在文件 velodyne_parser.h260 行定义.

◆ need_two_pt_correction_

bool apollo::drivers::velodyne::VelodyneParser::need_two_pt_correction_
protected

在文件 velodyne_parser.h259 行定义.

◆ sin_rot_table_

float apollo::drivers::velodyne::VelodyneParser::sin_rot_table_[ROTATION_MAX_UNITS]
protected

在文件 velodyne_parser.h254 行定义.


该类的文档由以下文件生成: