Skip to content

Commit 78ab3b8

Browse files
committed
refactor: Road and LaneSection, handle optional properties as such; enforce required properties
1 parent bea728f commit 78ab3b8

File tree

5 files changed

+77
-62
lines changed

5 files changed

+77
-62
lines changed

include/LaneSection.h

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ namespace odr
1010

1111
struct LaneSection
1212
{
13-
LaneSection(std::string road_id, double s0);
13+
LaneSection(double s0);
1414

1515
std::vector<Lane> get_lanes() const;
1616

@@ -19,8 +19,8 @@ struct LaneSection
1919
Lane get_lane(const int id) const;
2020
Lane get_lane(const double s, const double t) const;
2121

22-
std::string road_id = "";
23-
double s0 = 0;
22+
double s0;
23+
2424
std::map<int, Lane> id_to_lane;
2525
};
2626

include/Road.h

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
#include "RoadSignal.h"
1111

1212
#include <map>
13+
#include <optional>
1314
#include <set>
1415
#include <stdint.h>
1516
#include <string>
@@ -81,7 +82,7 @@ struct SpeedRecord
8182
class Road
8283
{
8384
public:
84-
Road(std::string id, double length, std::string junction, std::string name, bool left_hand_traffic = false);
85+
Road(std::string id, double length, std::string junction, bool left_hand_traffic = false, std::optional<std::string> name = std::nullopt);
8586

8687
std::vector<LaneSection> get_lanesections() const;
8788
std::vector<RoadObject> get_road_objects() const;
@@ -111,11 +112,12 @@ class Road
111112
std::set<double> approximate_lane_border_linear(const LaneKey& lane_key, double s_start, double s_end, double eps, bool outer = true) const;
112113
std::set<double> approximate_lane_border_linear(const LaneKey& lane_key, double eps, bool outer = true) const;
113114

114-
double length = 0;
115-
std::string id = "";
116-
std::string junction = "";
117-
std::string name = "";
118-
bool left_hand_traffic = false;
115+
std::string id;
116+
double length;
117+
std::string junction;
118+
bool left_hand_traffic;
119+
120+
std::optional<std::string> name;
119121

120122
RoadLink predecessor;
121123
RoadLink successor;

src/LaneSection.cpp

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,10 @@
66

77
namespace odr
88
{
9-
LaneSection::LaneSection(std::string road_id, double s0) : road_id(road_id), s0(s0) {}
9+
LaneSection::LaneSection(double s0) : s0(s0)
10+
{
11+
require_or_throw(s0 >= 0, "s {} < 0", s0);
12+
}
1013

1114
std::vector<Lane> LaneSection::get_lanes() const
1215
{

src/OpenDriveMap.cpp

Lines changed: 59 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -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

src/Road.cpp

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -64,9 +64,10 @@ std::vector<RoadSignal> Road::get_road_signals() const
6464
return get_map_values(this->id_to_signal);
6565
}
6666

67-
Road::Road(std::string id, double length, std::string junction, std::string name, bool left_hand_traffic) :
68-
length(length), id(id), junction(junction), name(name), left_hand_traffic(left_hand_traffic), ref_line(length)
67+
Road::Road(std::string id, double length, std::string junction, bool left_hand_traffic, std::optional<std::string> name) :
68+
id(id), length(length), junction(junction), left_hand_traffic(left_hand_traffic), name(name), ref_line(length)
6969
{
70+
require_or_throw(length > 0, "length {} <= 0", length);
7071
}
7172

7273
double Road::get_lanesection_s0(const double s) const
@@ -95,8 +96,6 @@ LaneSection Road::get_lanesection(const double s) const
9596

9697
double Road::get_lanesection_end(const LaneSection& lanesection) const
9798
{
98-
if (lanesection.road_id != this->id)
99-
log::error("LaneSection {} in Road #{}, not in Road #{}", lanesection.s0, lanesection.road_id, this->id);
10099
return this->get_lanesection_end(lanesection.s0);
101100
}
102101

0 commit comments

Comments
 (0)