148 lines
4.5 KiB
C++
148 lines
4.5 KiB
C++
// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
|
|
// de Barcelona (UAB).
|
|
//
|
|
// This work is licensed under the terms of the MIT license.
|
|
// For a copy, see <https://opensource.org/licenses/MIT>.
|
|
|
|
#include "carla/opendrive/parser/ProfilesParser.h"
|
|
|
|
#include "carla/road/MapBuilder.h"
|
|
|
|
#include <pugixml/pugixml.hpp>
|
|
|
|
namespace carla {
|
|
namespace opendrive {
|
|
namespace parser {
|
|
|
|
struct ElevationProfile {
|
|
carla::road::Road *road { nullptr };
|
|
double s { 0.0 };
|
|
double a { 0.0 };
|
|
double b { 0.0 };
|
|
double c { 0.0 };
|
|
double d { 0.0 };
|
|
};
|
|
|
|
struct LateralCrossfall {
|
|
std::string side { "both" };
|
|
};
|
|
|
|
struct LateralShape {
|
|
double t { 0.0 };
|
|
};
|
|
|
|
struct LateralProfile {
|
|
carla::road::Road *road { nullptr };
|
|
double s { 0.0 };
|
|
double a { 0.0 };
|
|
double b { 0.0 };
|
|
double c { 0.0 };
|
|
double d { 0.0 };
|
|
std::string type { "superelevation" };
|
|
LateralCrossfall cross;
|
|
LateralShape shape;
|
|
};
|
|
|
|
void ProfilesParser::Parse(
|
|
const pugi::xml_document &xml,
|
|
carla::road::MapBuilder &map_builder) {
|
|
|
|
std::vector<ElevationProfile> elevation_profile;
|
|
std::vector<LateralProfile> lateral_profile;
|
|
|
|
for (pugi::xml_node node_road : xml.child("OpenDRIVE").children("road")) {
|
|
|
|
// parse elevation profile
|
|
pugi::xml_node node_profile = node_road.child("elevationProfile");
|
|
uint64_t number_profiles = 0;
|
|
if (node_profile) {
|
|
// all geometry
|
|
for (pugi::xml_node node_elevation : node_profile.children("elevation")) {
|
|
ElevationProfile elev;
|
|
|
|
// get road id
|
|
road::RoadId road_id = node_road.attribute("id").as_uint();
|
|
elev.road = map_builder.GetRoad(road_id);
|
|
|
|
// get common properties
|
|
elev.s = node_elevation.attribute("s").as_double();
|
|
elev.a = node_elevation.attribute("a").as_double();
|
|
elev.b = node_elevation.attribute("b").as_double();
|
|
elev.c = node_elevation.attribute("c").as_double();
|
|
elev.d = node_elevation.attribute("d").as_double();
|
|
|
|
// add it
|
|
elevation_profile.emplace_back(elev);
|
|
number_profiles++;
|
|
}
|
|
}
|
|
// add a default profile if none is found
|
|
if(number_profiles == 0){
|
|
ElevationProfile elev;
|
|
road::RoadId road_id = node_road.attribute("id").as_uint();
|
|
elev.road = map_builder.GetRoad(road_id);
|
|
|
|
// get common properties
|
|
elev.s = 0;
|
|
elev.a = 0;
|
|
elev.b = 0;
|
|
elev.c = 0;
|
|
elev.d = 0;
|
|
|
|
// add it
|
|
elevation_profile.emplace_back(elev);
|
|
}
|
|
|
|
// parse lateral profile
|
|
node_profile = node_road.child("lateralProfile");
|
|
if (node_profile) {
|
|
for (pugi::xml_node node : node_profile.children()) {
|
|
LateralProfile lateral;
|
|
|
|
// get road id
|
|
road::RoadId road_id = node_road.attribute("id").as_uint();
|
|
lateral.road = map_builder.GetRoad(road_id);
|
|
|
|
// get common properties
|
|
lateral.s = node.attribute("s").as_double();
|
|
lateral.a = node.attribute("a").as_double();
|
|
lateral.b = node.attribute("b").as_double();
|
|
lateral.c = node.attribute("c").as_double();
|
|
lateral.d = node.attribute("d").as_double();
|
|
|
|
// handle different types
|
|
lateral.type = node.name();
|
|
if (lateral.type == "crossfall") {
|
|
lateral.cross.side = node.attribute("side").value();
|
|
} else if (lateral.type == "shape") {
|
|
lateral.shape.t = node.attribute("t").as_double();
|
|
}
|
|
|
|
// add it
|
|
lateral_profile.emplace_back(lateral);
|
|
}
|
|
}
|
|
}
|
|
|
|
// map_builder calls
|
|
for (auto const pro : elevation_profile) {
|
|
map_builder.AddRoadElevationProfile(pro.road, pro.s, pro.a, pro.b, pro.c, pro.d);
|
|
}
|
|
/// @todo: RoadInfo classes must be created to fit this information
|
|
// for (auto const pro : lateral_profile) {
|
|
// if (pro.type == "superelevation")
|
|
// map_builder.AddRoadLateralSuperElevation(pro.road, pro.s, pro.a,
|
|
// pro.b, pro.c, pro.d);
|
|
// else if (pro.type == "crossfall")
|
|
// map_builder.AddRoadLateralCrossfall(pro.road, pro.s, pro.a, pro.b,
|
|
// pro.c, pro.d, pro.cross.side);
|
|
// else if (pro.type == "shape")
|
|
// map_builder.AddRoadLateralShape(pro.road, pro.s, pro.a, pro.b, pro.c,
|
|
// pro.d, pro.shape.t);
|
|
// }
|
|
}
|
|
|
|
} // namespace parser
|
|
} // namespace opendrive
|
|
} // namespace carla
|