Apollo 10.0
自动驾驶开放平台
online_calibration.cc
浏览该文件的文档.
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
18
19namespace apollo {
20namespace drivers {
21namespace velodyne {
22
25
26int OnlineCalibration::decode(const std::shared_ptr<VelodyneScan>& scan_msgs) {
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}
121
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}
139
140void OnlineCalibration::dump(const std::string& file_path) {
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}
168
169} // namespace velodyne
170} // namespace drivers
171} // namespace apollo
std::map< int, LaserCorrection > laser_corrections_
Definition calibration.h:70
void dump(const std::string &file_path)
int decode(const std::shared_ptr< VelodyneScan > &scan_msgs)
#define AERROR
Definition log.h:44
#define AINFO
Definition log.h:42
constexpr double DEGRESS_TO_RADIANS
class register implement
Definition arena_queue.h:37
correction values for a single laser
Definition calibration.h:43
float sin_rot_correction
cached sine of rot_correction
Definition calibration.h:60
float cos_rot_correction
cached values calculated when the calibration file is read
Definition calibration.h:59
float cos_vert_correction
cached cosine of vert_correction
Definition calibration.h:61
float rot_correction
parameters in db.xml
Definition calibration.h:45
int laser_ring
ring number for this laser
Definition calibration.h:64
float sin_vert_correction
cached sine of vert_correction
Definition calibration.h:62