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

#include <online_calibration.h>

apollo::drivers::velodyne::OnlineCalibration 的协作图:

Public 成员函数

 OnlineCalibration ()
 
 ~OnlineCalibration ()
 
int decode (const std::shared_ptr< VelodyneScan > &scan_msgs)
 
void dump (const std::string &file_path)
 
void get_unit_index ()
 
bool inited () const
 
Calibration calibration () const
 

详细描述

在文件 online_calibration.h37 行定义.

构造及析构函数说明

◆ OnlineCalibration()

apollo::drivers::velodyne::OnlineCalibration::OnlineCalibration ( )
inline

在文件 online_calibration.h39 行定义.

39{}

◆ ~OnlineCalibration()

apollo::drivers::velodyne::OnlineCalibration::~OnlineCalibration ( )
inline

在文件 online_calibration.h40 行定义.

40{}

成员函数说明

◆ calibration()

Calibration apollo::drivers::velodyne::OnlineCalibration::calibration ( ) const
inline

在文件 online_calibration.h46 行定义.

46{ return calibration_; }

◆ decode()

int apollo::drivers::velodyne::OnlineCalibration::decode ( const std::shared_ptr< VelodyneScan > &  scan_msgs)

在文件 online_calibration.cc26 行定义.

26 {
27 if (inited_) {
28 return 0;
29 }
30 for (auto& packet : scan_msgs->firing_pkts()) {
31 if (packet.data().size() < 1206) {
32 AERROR << "Ivalid packet data size, expect 1206, actually "
33 << packet.data().size();
34 return -1;
35 }
36 uint8_t* data =
37 reinterpret_cast<uint8_t*>(const_cast<char*>(packet.data().c_str()));
38 status_types_.emplace_back(data[1204]);
39 status_values_.emplace_back(data[1205]);
40 }
41 // read calibration when get 2s packet
42 if (status_types_.size() < 5789 * 2) {
43 AINFO << "Wait for more scan msgs";
44 return -1;
45 }
47 int unit_size = static_cast<int>(unit_indexs_.size());
48 if (unit_size < 2) {
49 // can not find two unit# index, may be lost packet
50 AINFO << "unit count less than 2, maybe lost packets";
51 return -1;
52 }
53
54 if (unit_indexs_[unit_size - 1] - unit_indexs_[unit_size - 2] !=
55 65 * 64) { // 64 lasers
56 // lost packet
57 AERROR << "two unit distance is wrong";
58 return -1;
59 }
60
61 int start_index = unit_indexs_[unit_size - 2];
62 for (int i = 0; i < 64; ++i) {
63 LaserCorrection laser_correction;
64
65 int index_16 = start_index + i * 64 + 16;
66 int index_32 = start_index + i * 64 + 32;
67 int index_48 = start_index + i * 64 + 48;
68
69 laser_correction.laser_ring = status_values_[index_16];
70
71 laser_correction.vert_correction =
72 *reinterpret_cast<int16_t*>(&status_values_[index_16 + 1]) / 100.0f *
73 static_cast<float>(DEGRESS_TO_RADIANS);
74 laser_correction.rot_correction =
75 *reinterpret_cast<int16_t*>(&status_values_[index_16 + 3]) / 100.0f *
76 static_cast<float>(DEGRESS_TO_RADIANS);
77 laser_correction.dist_correction =
78 *reinterpret_cast<int16_t*>(&status_values_[index_16 + 5]) / 10.0f /
79 100.0f; // to meter
80 laser_correction.dist_correction_x =
81 *reinterpret_cast<int16_t*>(&status_values_[index_32]) / 10.0f /
82 100.0f; // to meter
83 laser_correction.dist_correction_y =
84 *reinterpret_cast<int16_t*>(&status_values_[index_32 + 2]) / 10.0f /
85 100.0f; // to meter
86 laser_correction.vert_offset_correction =
87 *reinterpret_cast<int16_t*>(&status_values_[index_32 + 4]) / 10.0f /
88 100.0f; // to meter
89 laser_correction.horiz_offset_correction =
90 static_cast<int16_t>(static_cast<int16_t>(status_values_[index_48])
91 << 8 |
92 status_values_[index_32 + 6]) /
93 10.0f / 100.0f; // to meter
94 laser_correction.focal_distance =
95 *reinterpret_cast<int16_t*>(&status_values_[index_48 + 1]) / 10.0f /
96 100.0f; // to meter
97 laser_correction.focal_slope =
98 *reinterpret_cast<int16_t*>(&status_values_[index_48 + 3]) /
99 10.0f; // to meter
100 laser_correction.max_intensity = status_values_[index_48 + 6];
101 laser_correction.min_intensity = status_values_[index_48 + 5];
102
103 laser_correction.cos_rot_correction = cosf(laser_correction.rot_correction);
104 laser_correction.sin_rot_correction = sinf(laser_correction.rot_correction);
105 laser_correction.cos_vert_correction =
106 cosf(laser_correction.vert_correction);
107 laser_correction.sin_vert_correction =
108 sinf(laser_correction.vert_correction);
109 laser_correction.focal_offset =
110 256.0f * static_cast<float>(
111 std::pow(1 - laser_correction.focal_distance / 13100, 2));
112
113 calibration_.laser_corrections_[laser_correction.laser_ring] =
114 laser_correction;
115 }
116 calibration_.num_lasers_ = 64;
117 calibration_.initialized_ = true;
118 inited_ = true;
119 return 0;
120}
std::map< int, LaserCorrection > laser_corrections_
Definition calibration.h:70
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42
constexpr double DEGRESS_TO_RADIANS

◆ dump()

void apollo::drivers::velodyne::OnlineCalibration::dump ( const std::string &  file_path)

在文件 online_calibration.cc140 行定义.

140 {
141 if (!inited_) {
142 AERROR << "Please decode calibraion info first";
143 return;
144 }
145 std::ofstream ofs(file_path.c_str(), std::ios::out);
146 ofs << "lasers:" << std::endl;
147 for (auto& correction : calibration_.laser_corrections_) {
148 ofs << "- {";
149 ofs << "dist_correction: " << correction.second.dist_correction << ", ";
150 ofs << "dist_correction_x: " << correction.second.dist_correction_x << ", ";
151 ofs << "dist_correction_y: " << correction.second.dist_correction_y << ", ";
152 ofs << "focal_distance: " << correction.second.focal_distance << ", ";
153 ofs << "focal_slope: " << correction.second.focal_slope << ", ";
154 ofs << "horiz_offset_correction: "
155 << correction.second.horiz_offset_correction << ", ";
156 ofs << "laser_id: " << correction.second.laser_ring << ", ";
157 ofs << "max_intensity: " << correction.second.max_intensity << ", ";
158 ofs << "min_intensity: " << correction.second.min_intensity << ", ";
159 ofs << "rot_correction: " << correction.second.rot_correction << ", ";
160 ofs << "vert_correction: " << correction.second.vert_correction << ", ";
161 ofs << "vert_offset_correction: "
162 << correction.second.vert_offset_correction;
163 ofs << "}" << std::endl;
164 }
165 ofs << "num_lasers: " << calibration_.num_lasers_ << std::endl;
166 ofs.close();
167}

◆ get_unit_index()

void apollo::drivers::velodyne::OnlineCalibration::get_unit_index ( )

在文件 online_calibration.cc122 行定义.

122 {
123 int size = static_cast<int>(status_values_.size());
124 // simple check only for value, maybe need more check fro status type
125 int start_index = 0;
126 if (unit_indexs_.size() > 0) {
127 start_index = unit_indexs_.back() + 5;
128 }
129 for (; start_index < size - 5; ++start_index) {
130 if (status_values_[start_index] == 85 // "U"
131 && status_values_[start_index + 1] == 78 // "N"
132 && status_values_[start_index + 2] == 73 // "I"
133 && status_values_[start_index + 3] == 84 // "T"
134 && status_values_[start_index + 4] == 35) { // "#"
135 unit_indexs_.emplace_back(start_index);
136 }
137 }
138}

◆ inited()

bool apollo::drivers::velodyne::OnlineCalibration::inited ( ) const
inline

在文件 online_calibration.h45 行定义.

45{ return inited_; }

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