@@ -83,18 +83,23 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
8383 continue ;
8484 }
8585
86- const double length = road_node.attribute (" length" ).as_double (NAN);
87- if (std::isnan (length) || length < 0 )
88- {
89- log::error (" {}: length {} < 0" , node_path (road_node), length);
90- continue ;
91- }
92-
9386 std::string rule_str = std::string (road_node.attribute (" rule" ).as_string (" RHT" ));
9487 std::transform (rule_str.begin (), rule_str.end (), rule_str.begin (), [](unsigned char c) { return std::tolower (c); });
95- const bool is_left_hand_traffic = (rule_str == " lht" );
9688
97- Road road (road_id, length, road_node.attribute (" junction" ).as_string (" " ), road_node.attribute (" name" ).as_string (" " ), is_left_hand_traffic);
89+ std::optional<Road> road;
90+ try
91+ {
92+ road.emplace (road_id,
93+ road_node.attribute (" length" ).as_double (NAN),
94+ road_node.attribute (" junction" ).as_string (" " ),
95+ rule_str == " lht" ,
96+ try_get_attribute<std::string>(road_node, " name" ));
97+ }
98+ catch (const std::exception& ex)
99+ {
100+ log::warn (" {}: {}" , node_path (road_node), ex.what ());
101+ continue ;
102+ }
98103
99104 // parse road links
100105 for (const bool is_predecessor : {true , false })
@@ -103,7 +108,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
103108 is_predecessor ? road_node.child (" link" ).child (" predecessor" ) : road_node.child (" link" ).child (" successor" );
104109 if (road_link_node)
105110 {
106- RoadLink& link = is_predecessor ? road. predecessor : road. successor ;
111+ RoadLink& link = is_predecessor ? road-> predecessor : road-> successor ;
107112 link.id = road_link_node.attribute (" elementId" ).as_string (" " );
108113
109114 const std::string type_str = road_link_node.attribute (" elementType" ).as_string (" " );
@@ -138,7 +143,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
138143 const std::string road_neighbor_id = road_neighbor_node.attribute (" elementId" ).as_string (" " );
139144 const std::string road_neighbor_side = road_neighbor_node.attribute (" side" ).as_string (" " );
140145 const std::string road_neighbor_direction = road_neighbor_node.attribute (" direction" ).as_string (" " );
141- road. neighbors .emplace_back (road_neighbor_id, road_neighbor_side, road_neighbor_direction);
146+ road-> neighbors .emplace_back (road_neighbor_id, road_neighbor_side, road_neighbor_direction);
142147 }
143148
144149 // parse road type and speed
@@ -151,13 +156,13 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
151156 log::warn (" {}: s < 0" , node_path (road_type_node));
152157 continue ;
153158 }
154- road. s_to_type [s] = type;
159+ road-> s_to_type [s] = type;
155160
156161 if (const pugi::xml_node node = road_type_node.child (" speed" ))
157162 {
158163 const std::string speed_record_max = node.attribute (" max" ).as_string (" " );
159164 const std::string speed_record_unit = node.attribute (" unit" ).as_string (" " );
160- road. s_to_speed .emplace (s, SpeedRecord (speed_record_max, speed_record_unit));
165+ road-> s_to_speed .emplace (s, SpeedRecord (speed_record_max, speed_record_unit));
161166 }
162167 }
163168
@@ -197,7 +202,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
197202 const std::string geometry_type = geometry_node.name ();
198203 if (geometry_type == " line" )
199204 {
200- road. ref_line .s0_to_geometry [s0] = std::make_unique<Line>(s0, x0, y0, hdg0, length);
205+ road-> ref_line .s0_to_geometry [s0] = std::make_unique<Line>(s0, x0, y0, hdg0, length);
201206 }
202207 else if (geometry_type == " spiral" )
203208 {
@@ -211,24 +216,24 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
211216 }
212217 if (!fix_spiral_edge_cases)
213218 {
214- road. ref_line .s0_to_geometry [s0] = std::make_unique<Spiral>(s0, x0, y0, hdg0, length, curv_start, curv_end);
219+ road-> ref_line .s0_to_geometry [s0] = std::make_unique<Spiral>(s0, x0, y0, hdg0, length, curv_start, curv_end);
215220 }
216221 else
217222 {
218223 if (std::abs (curv_start) < 1e-6 && std::abs (curv_end) < 1e-6 )
219224 {
220225 // In effect a line
221- road. ref_line .s0_to_geometry [s0] = std::make_unique<Line>(s0, x0, y0, hdg0, length);
226+ road-> ref_line .s0_to_geometry [s0] = std::make_unique<Line>(s0, x0, y0, hdg0, length);
222227 }
223228 else if (std::abs (curv_end - curv_start) < 1e-6 )
224229 {
225230 // In effect an arc
226- road. ref_line .s0_to_geometry [s0] = std::make_unique<Arc>(s0, x0, y0, hdg0, length, curv_start);
231+ road-> ref_line .s0_to_geometry [s0] = std::make_unique<Arc>(s0, x0, y0, hdg0, length, curv_start);
227232 }
228233 else
229234 {
230235 // True spiral
231- road. ref_line .s0_to_geometry [s0] = std::make_unique<Spiral>(s0, x0, y0, hdg0, length, curv_start, curv_end);
236+ road-> ref_line .s0_to_geometry [s0] = std::make_unique<Spiral>(s0, x0, y0, hdg0, length, curv_start, curv_end);
232237 }
233238 }
234239 }
@@ -241,7 +246,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
241246 invalid_geometry = true ;
242247 continue ;
243248 }
244- road. ref_line .s0_to_geometry [s0] = std::make_unique<Arc>(s0, x0, y0, hdg0, length, curvature);
249+ road-> ref_line .s0_to_geometry [s0] = std::make_unique<Arc>(s0, x0, y0, hdg0, length, curvature);
245250 }
246251 else if (geometry_type == " paramPoly3" )
247252 {
@@ -279,7 +284,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
279284 if (pRange_str == " arclength" )
280285 pRange_normalized = false ;
281286 }
282- road. ref_line .s0_to_geometry [s0] =
287+ road-> ref_line .s0_to_geometry [s0] =
283288 std::make_unique<ParamPoly3>(s0, x0, y0, hdg0, length, aU, bU, cU, dU, aV, bV, cV, dV, pRange_normalized);
284289 }
285290 else
@@ -296,9 +301,9 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
296301 }
297302
298303 std::map<std::string /* x path query*/ , CubicProfile&> cubic_profile_fields{
299- {" .//elevationProfile//elevation" , road. ref_line .elevation_profile }, {" .//lanes//laneOffset" , road. lane_offset }};
304+ {" .//elevationProfile//elevation" , road-> ref_line .elevation_profile }, {" .//lanes//laneOffset" , road-> lane_offset }};
300305 if (with_lateral_profile)
301- cubic_profile_fields.insert ({" .//lateralProfile//superelevation" , road. superelevation });
306+ cubic_profile_fields.insert ({" .//lateralProfile//superelevation" , road-> superelevation });
302307
303308 // parse elevation profiles, lane offsets, superelevation
304309 bool invalid_cubic = false ;
@@ -359,17 +364,17 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
359364 continue ;
360365 }
361366
362- road. crossfall .segments .emplace (s0, CubicPoly (a, b, c, d, s0));
367+ road-> crossfall .segments .emplace (s0, CubicPoly (a, b, c, d, s0));
363368 if (const pugi::xml_attribute side = crossfall_node.attribute (" side" ))
364369 {
365370 std::string side_str = side.as_string (" " );
366371 std::transform (side_str.begin (), side_str.end (), side_str.begin (), [](unsigned char c) { return std::tolower (c); });
367372 if (side_str == " left" )
368- road. crossfall .s_to_side [s0] = Crossfall::Side::Left;
373+ road-> crossfall .s_to_side [s0] = Crossfall::Side::Left;
369374 else if (side_str == " right" )
370- road. crossfall .s_to_side [s0] = Crossfall::Side::Right;
375+ road-> crossfall .s_to_side [s0] = Crossfall::Side::Right;
371376 else // default to 'both'
372- road. crossfall .s_to_side [s0] = Crossfall::Side::Both;
377+ road-> crossfall .s_to_side [s0] = Crossfall::Side::Both;
373378 }
374379 }
375380
@@ -384,25 +389,31 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
384389 bool invalid_lanesection = false ;
385390 for (const pugi::xml_node lanesection_node : road_node.child (" lanes" ).children (" laneSection" ))
386391 {
387- const double s0 = lanesection_node. attribute ( " s " ). as_double (NAN) ;
388- if ( std::isnan (s0) || s0 < 0 )
392+ std::optional<LaneSection> lanesection ;
393+ try
389394 {
390- log::error (" {}: s < 0" , node_path (lanesection_node));
395+ lanesection.emplace (lanesection_node.attribute (" s" ).as_double (NAN));
396+ }
397+ catch (const std::exception& ex)
398+ {
399+ log::warn (" {}: {}" , node_path (lanesection_node), ex.what ());
391400 invalid_lanesection = true ;
392401 continue ;
393402 }
394403
395- LaneSection& lanesection = road.s_to_lanesection .emplace (s0, LaneSection (road_id, s0)).first ->second ;
396-
397404 for (const pugi::xpath_node lane_xpath_node : lanesection_node.select_nodes (" .//lane" ))
398405 {
399406 const pugi::xml_node lane_node = lane_xpath_node.node ();
400407 const int lane_id = lane_node.attribute (" id" ).as_int (0 );
401408
402- odr::check (!lane_node.child (" border" ), " Road #{} LaneSection {} Lane #{}: border definitions not supported" , road_id, s0, lane_id);
409+ odr::check (!lane_node.child (" border" ),
410+ " Road #{} LaneSection {} Lane #{}: border definitions not supported" ,
411+ road_id,
412+ lanesection->s0 ,
413+ lane_id);
403414
404415 Lane& lane =
405- lanesection. id_to_lane
416+ lanesection-> id_to_lane
406417 .emplace (lane_id, Lane (lane_id, lane_node.attribute (" type" ).as_string (" " ), lane_node.attribute (" level" ).as_bool (false )))
407418 .first ->second ;
408419
@@ -432,7 +443,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
432443 continue ;
433444 }
434445
435- CubicPoly width_poly (a, b, c, d, s0 + s_offset);
446+ CubicPoly width_poly (a, b, c, d, lanesection-> s0 + s_offset);
436447
437448 // OpenDRIVE Format Specification, Rev. 1.4, 3.3.1 General:
438449 // "The reference line itself is defined as lane zero and must not have a width entry (i.e. its width must always be 0.0)."
@@ -442,7 +453,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
442453 width_poly.set_zero ();
443454 }
444455
445- lane.lane_width .segments .emplace (s0 + s_offset, width_poly);
456+ lane.lane_width .segments .emplace (lanesection-> s0 + s_offset, width_poly);
446457 }
447458
448459 if (with_lane_height)
@@ -464,7 +475,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
464475 continue ;
465476 }
466477
467- lane.s_to_height_offset .emplace (s0 + s_offset, HeightOffset (inner, outer));
478+ lane.s_to_height_offset .emplace (lanesection-> s0 + s_offset, HeightOffset (inner, outer));
468479 }
469480 }
470481
@@ -528,18 +539,18 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
528539 }
529540 }
530541
531- lane.s_to_roadmark .emplace (s0 + roadmark->s_offset , std::move (*roadmark));
542+ lane.s_to_roadmark .emplace (lanesection-> s0 + roadmark->s_offset , std::move (*roadmark));
532543 }
533544 }
534545
535546 // derive lane borders from lane widths
536- const auto id_lane_iter0 = lanesection. id_to_lane .find (0 );
537- if (id_lane_iter0 == lanesection. id_to_lane .end ())
547+ const auto id_lane_iter0 = lanesection-> id_to_lane .find (0 );
548+ if (id_lane_iter0 == lanesection-> id_to_lane .end ())
538549 throw std::runtime_error (" lane section does not have lane #0" );
539550
540551 // iterate from lane #1 towards +inf
541552 const auto id_lane_iter1 = std::next (id_lane_iter0);
542- for (auto iter = id_lane_iter1; iter != lanesection. id_to_lane .end (); iter++)
553+ for (auto iter = id_lane_iter1; iter != lanesection-> id_to_lane .end (); iter++)
543554 {
544555 if (iter == id_lane_iter1)
545556 iter->second .outer_border = iter->second .lane_width ;
@@ -551,7 +562,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
551562 // "For a reverse iterator r constructed from an iterator i, the relationship &*r == &*(i - 1) is always true"
552563 // The reverse iterator points to the element that is one before the element referred by the id_lane_iter0!
553564 const std::map<int , Lane>::reverse_iterator r_id_lane_iter1 (id_lane_iter0);
554- for (auto r_iter = r_id_lane_iter1; r_iter != lanesection. id_to_lane .rend (); r_iter++)
565+ for (auto r_iter = r_id_lane_iter1; r_iter != lanesection-> id_to_lane .rend (); r_iter++)
555566 {
556567 if (r_iter == r_id_lane_iter1)
557568 r_iter->second .outer_border = r_iter->second .lane_width .negate ();
@@ -561,9 +572,9 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
561572
562573 // OpenDRIVE Format Specification, Rev. 1.4, 3.3.2 Lane Offset:
563574 // "... lane 0 may be offset using a cubic polynom"
564- for (auto & id_lane : lanesection. id_to_lane )
575+ for (auto & id_lane : lanesection-> id_to_lane )
565576 {
566- id_lane.second .outer_border = id_lane.second .outer_border .add (road. lane_offset );
577+ id_lane.second .outer_border = id_lane.second .outer_border .add (road-> lane_offset );
567578 }
568579 }
569580 if (invalid_lanesection)
@@ -581,7 +592,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
581592 for (const pugi::xml_node object_node : road_node.child (" objects" ).children (" object" ))
582593 {
583594 const std::string object_id = object_node.attribute (" id" ).as_string (" " );
584- if (road. id_to_object .find (object_id) != road. id_to_object .end ())
595+ if (road-> id_to_object .find (object_id) != road-> id_to_object .end ())
585596 {
586597 log::warn (" {}: duplicate Object #{}" , node_path (object_node), object_id);
587598 continue ;
@@ -698,7 +709,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
698709 road_object->lane_validities .emplace_back (from_lane, to_lane);
699710 }
700711
701- road. id_to_object .emplace (object_id, std::move (*road_object));
712+ road-> id_to_object .emplace (object_id, std::move (*road_object));
702713 }
703714 }
704715 // parse signals
@@ -707,7 +718,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
707718 for (const pugi::xml_node signal_node : road_node.child (" signals" ).children (" signal" ))
708719 {
709720 const std::string signal_id = signal_node.attribute (" id" ).as_string (" " );
710- if (road. id_to_signal .find (signal_id) != road. id_to_signal .end ())
721+ if (road-> id_to_signal .find (signal_id) != road-> id_to_signal .end ())
711722 {
712723 log::warn (" {}: duplicate Signal #{}" , node_path (signal_node), signal_id);
713724 continue ;
@@ -754,11 +765,11 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
754765 road_signal->lane_validities .emplace_back (from_lane, to_lane);
755766 }
756767
757- road. id_to_signal .emplace (signal_id, std::move (*road_signal));
768+ road-> id_to_signal .emplace (signal_id, std::move (*road_signal));
758769 }
759770 }
760771
761- this ->id_to_road .emplace (road. id , std::move (road));
772+ this ->id_to_road .emplace (road-> id , std::move (* road));
762773 }
763774
764775 // Junctions
0 commit comments