CARLA
GeometryParser.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
8 
10 
11 #include <pugixml/pugixml.hpp>
12 
13 namespace carla {
14 namespace opendrive {
15 namespace parser {
16 
18 
19  struct GeometryArc {
20  double curvature { 0.0 };
21  };
22 
23  struct GeometrySpiral {
24  double curvStart { 0.0 };
25  double curvEnd { 0.0 };
26  };
27 
28  struct GeometryPoly3 {
29  double a { 0.0 };
30  double b { 0.0 };
31  double c { 0.0 };
32  double d { 0.0 };
33  };
34 
36  double aU { 0.0 };
37  double bU { 0.0 };
38  double cU { 0.0 };
39  double dU { 0.0 };
40  double aV { 0.0 };
41  double bV { 0.0 };
42  double cV { 0.0 };
43  double dV { 0.0 };
44  std::string p_range { "arcLength" };
45  };
46 
47  struct Geometry {
48  RoadId road_id { 0u };
49  double s { 0.0 };
50  double x { 0.0 };
51  double y { 0.0 };
52  double hdg { 0.0 };
53  double length { 0.0 };
54  std::string type { "line" };
59  };
60 
62  const pugi::xml_document &xml,
63  carla::road::MapBuilder &map_builder) {
64 
65  std::vector<Geometry> geometry;
66 
67  for (pugi::xml_node node_road : xml.child("OpenDRIVE").children("road")) {
68 
69  // parse plan view
70  pugi::xml_node node_plan_view = node_road.child("planView");
71  if (node_plan_view) {
72  // all geometry
73  for (pugi::xml_node node_geo : node_plan_view.children("geometry")) {
74  Geometry geo;
75 
76  // get road id
77  geo.road_id = node_road.attribute("id").as_uint();
78 
79  // get common properties
80  geo.s = node_geo.attribute("s").as_double();
81  geo.x = node_geo.attribute("x").as_double();
82  geo.y = node_geo.attribute("y").as_double();
83  geo.hdg = node_geo.attribute("hdg").as_double();
84  geo.length = node_geo.attribute("length").as_double();
85 
86  // check geometry type
87  pugi::xml_node node = node_geo.first_child();
88  geo.type = node.name();
89  if (geo.type == "arc") {
90  geo.arc.curvature = node.attribute("curvature").as_double();
91  } else if (geo.type == "spiral") {
92  geo.spiral.curvStart = node.attribute("curvStart").as_double();
93  geo.spiral.curvEnd = node.attribute("curvEnd").as_double();
94  } else if (geo.type == "poly3") {
95  geo.poly3.a = node.attribute("a").as_double();
96  geo.poly3.b = node.attribute("b").as_double();
97  geo.poly3.c = node.attribute("c").as_double();
98  geo.poly3.d = node.attribute("d").as_double();
99  } else if (geo.type == "paramPoly3") {
100  geo.param_poly3.aU = node.attribute("aU").as_double();
101  geo.param_poly3.bU = node.attribute("bU").as_double();
102  geo.param_poly3.cU = node.attribute("cU").as_double();
103  geo.param_poly3.dU = node.attribute("dU").as_double();
104  geo.param_poly3.aV = node.attribute("aV").as_double();
105  geo.param_poly3.bV = node.attribute("bV").as_double();
106  geo.param_poly3.cV = node.attribute("cV").as_double();
107  geo.param_poly3.dV = node.attribute("dV").as_double();
108  geo.param_poly3.p_range = node.attribute("pRange").value();
109  }
110 
111  // add it
112  geometry.emplace_back(geo);
113  }
114  }
115  }
116 
117  // map_builder calls
118  for (auto const geo : geometry) {
119  carla::road::Road *road = map_builder.GetRoad(geo.road_id);
120  if (geo.type == "line") {
121  map_builder.AddRoadGeometryLine(road, geo.s, geo.x, geo.y, geo.hdg, geo.length);
122  } else if (geo.type == "arc") {
123  map_builder.AddRoadGeometryArc(road, geo.s, geo.x, geo.y, geo.hdg, geo.length, geo.arc.curvature);
124  } else if (geo.type == "spiral") {
125  map_builder.AddRoadGeometrySpiral(road,
126  geo.s,
127  geo.x,
128  geo.y,
129  geo.hdg,
130  geo.length,
131  geo.spiral.curvStart,
132  geo.spiral.curvEnd);
133  } else if (geo.type == "poly3") {
134  map_builder.AddRoadGeometryPoly3(road,
135  geo.s,
136  geo.x,
137  geo.y,
138  geo.hdg,
139  geo.length,
140  geo.poly3.a,
141  geo.poly3.b,
142  geo.poly3.c,
143  geo.poly3.d);
144  } else if (geo.type == "paramPoly3") {
145  map_builder.AddRoadGeometryParamPoly3(road,
146  geo.s,
147  geo.x,
148  geo.y,
149  geo.hdg,
150  geo.length,
151  geo.param_poly3.aU,
152  geo.param_poly3.bU,
153  geo.param_poly3.cU,
154  geo.param_poly3.dU,
155  geo.param_poly3.aV,
156  geo.param_poly3.bV,
157  geo.param_poly3.cV,
158  geo.param_poly3.dV,
159  geo.param_poly3.p_range);
160  }
161  }
162  }
163 
164 } // namespace parser
165 } // namespace opendrive
166 } // namespace carla
const char_t * value() const
Definition: pugixml.cpp:5215
double as_double(double def=0) const
Definition: pugixml.cpp:5178
void AddRoadGeometryParamPoly3(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length, const double aU, const double bU, const double cU, const double dU, const double aV, const double bV, const double cV, const double dV, const std::string p_range)
Definition: MapBuilder.cpp:523
Road * GetRoad(const RoadId road_id)
Definition: MapBuilder.cpp:603
uint32_t RoadId
Definition: RoadTypes.h:15
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
xml_attribute attribute(const char_t *name) const
Definition: pugixml.cpp:5500
void AddRoadGeometrySpiral(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length, const double curvStart, const double curvEnd)
Definition: MapBuilder.cpp:472
void AddRoadGeometryArc(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length, const double curvature)
Definition: MapBuilder.cpp:451
xml_node child(const char_t *name) const
Definition: pugixml.cpp:5490
xml_node first_child() const
Definition: pugixml.cpp:5622
const char_t * name() const
Definition: pugixml.cpp:5475
void AddRoadGeometryPoly3(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length, const double a, const double b, const double c, const double d)
Definition: MapBuilder.cpp:496
static void Parse(const pugi::xml_document &xml, carla::road::MapBuilder &map_builder)
void AddRoadGeometryLine(carla::road::Road *road, const double s, const double x, const double y, const double hdg, const double length)
Definition: MapBuilder.cpp:411
xml_object_range< xml_node_iterator > children() const
Definition: pugixml.cpp:5425