19 #include <yaml-cpp/yaml.h>
21 #ifdef HAVE_NEW_YAMLCPP
31 #endif // HAVE_NEW_YAMLCPP
47 "two_pt_correction_available";
59 std::pair<int, LaserCorrection>& correction)
65 #ifdef HAVE_NEW_YAMLCPP
68 correction.second.two_pt_correction_available;
71 *pName >> correction.second.two_pt_correction_available;
74 correction.second.two_pt_correction_available =
false;
78 #ifdef HAVE_NEW_YAMLCPP
81 correction.second.horiz_offset_correction;
84 *pName >> correction.second.horiz_offset_correction;
87 correction.second.horiz_offset_correction = 0;
89 const YAML::Node * max_intensity_node = NULL;
90 #ifdef HAVE_NEW_YAMLCPP
92 const YAML::Node max_intensity_node_ref = node[
MAX_INTENSITY];
93 max_intensity_node = &max_intensity_node_ref;
97 max_intensity_node = pName;
99 if (max_intensity_node) {
100 float max_intensity_float;
101 *max_intensity_node >> max_intensity_float;
102 correction.second.max_intensity = floor(max_intensity_float);
105 correction.second.max_intensity = 255;
108 const YAML::Node * min_intensity_node = NULL;
109 #ifdef HAVE_NEW_YAMLCPP
111 const YAML::Node min_intensity_node_ref = node[
MIN_INTENSITY];
112 min_intensity_node = &min_intensity_node_ref;
116 min_intensity_node = pName;
118 if (min_intensity_node) {
119 float min_intensity_float;
120 *min_intensity_node >> min_intensity_float;
121 correction.second.min_intensity = floor(min_intensity_float);
124 correction.second.min_intensity = 0;
127 node[
FOCAL_SLOPE] >> correction.second.focal_slope;
130 correction.second.cos_rot_correction =
131 cosf(correction.second.rot_correction);
132 correction.second.sin_rot_correction =
133 sinf(correction.second.rot_correction);
134 correction.second.cos_vert_correction =
135 cosf(correction.second.vert_correction);
136 correction.second.sin_vert_correction =
137 sinf(correction.second.vert_correction);
139 correction.second.laser_ring = 0;
147 float distance_resolution_m;
149 const YAML::Node& lasers = node[
LASERS];
152 calibration.distance_resolution_m = distance_resolution_m;
154 for (
int i = 0; i < num_lasers; i++) {
155 std::pair<int, LaserCorrection> correction;
156 lasers[i] >> correction;
157 const int index = correction.first;
163 calibration.laser_corrections_map.insert(correction);
170 double next_angle = -std::numeric_limits<double>::infinity();
171 for (
int ring = 0; ring < num_lasers; ++ring) {
174 double min_seen = std::numeric_limits<double>::infinity();
175 int next_index = num_lasers;
176 for (
int j = 0; j < num_lasers; ++j) {
185 if (next_index < num_lasers) {
188 calibration.laser_corrections[next_index].laser_ring = ring;
189 next_angle = min_seen;
191 ROS_INFO(
"laser_ring[%2u] = %2u, angle = %+.6f",
192 next_index, ring, next_angle);
199 const std::pair<int, LaserCorrection> correction)
201 out << YAML::BeginMap;
202 out << YAML::Key <<
LASER_ID << YAML::Value << correction.first;
204 YAML::Value << correction.second.rot_correction;
206 YAML::Value << correction.second.vert_correction;
208 YAML::Value << correction.second.dist_correction;
210 YAML::Value << correction.second.two_pt_correction_available;
212 YAML::Value << correction.second.dist_correction_x;
214 YAML::Value << correction.second.dist_correction_y;
216 YAML::Value << correction.second.vert_offset_correction;
218 YAML::Value << correction.second.horiz_offset_correction;
220 YAML::Value << correction.second.max_intensity;
222 YAML::Value << correction.second.min_intensity;
224 YAML::Value << correction.second.focal_distance;
226 YAML::Value << correction.second.focal_slope;
231 YAML::Emitter&
operator <<
234 out << YAML::BeginMap;
236 YAML::Value <<
calibration.laser_corrections.size();
239 out << YAML::Key <<
LASERS << YAML::Value << YAML::BeginSeq;
240 for (std::map<int, LaserCorrection>::const_iterator
242 it !=
calibration.laser_corrections_map.end(); it++)
252 std::ifstream fin(calibration_file.c_str());
253 if (!fin.is_open()) {
260 #ifdef HAVE_NEW_YAMLCPP
262 doc = YAML::LoadFile(calibration_file);
265 parser.GetNextDocument(doc);
268 }
catch (YAML::Exception &e) {
269 std::cerr <<
"YAML Exception: " << e.what() << std::endl;
276 std::ofstream fout(calibration_file.c_str());