223 lines
8.8 KiB
C++
223 lines
8.8 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/LaneParser.h"
|
|
|
|
#include "carla/road/MapBuilder.h"
|
|
|
|
#include <pugixml/pugixml.hpp>
|
|
|
|
namespace carla {
|
|
namespace opendrive {
|
|
namespace parser {
|
|
|
|
static void ParseLanes(
|
|
road::RoadId road_id,
|
|
double s,
|
|
const pugi::xml_node &parent_node,
|
|
carla::road::MapBuilder &map_builder) {
|
|
for (pugi::xml_node lane_node : parent_node.children("lane")) {
|
|
|
|
road::LaneId lane_id = lane_node.attribute("id").as_int();
|
|
|
|
road::Lane *lane = map_builder.GetLane(road_id, lane_id, s);
|
|
|
|
// Lane Width
|
|
int width_count = 0;
|
|
for (pugi::xml_node lane_width_node : lane_node.children("width")) {
|
|
const double s_offset = lane_width_node.attribute("sOffset").as_double();
|
|
const double a = lane_width_node.attribute("a").as_double();
|
|
const double b = lane_width_node.attribute("b").as_double();
|
|
const double c = lane_width_node.attribute("c").as_double();
|
|
const double d = lane_width_node.attribute("d").as_double();
|
|
|
|
// Call Map builder create Lane Width function
|
|
map_builder.CreateLaneWidth(lane, s_offset + s, a, b, c, d);
|
|
width_count++;
|
|
}
|
|
if (width_count == 0 && lane->GetId() != 0) {
|
|
map_builder.CreateLaneWidth(lane, s, 0.0, 0.0, 0.0, 0.0);
|
|
std::cout << "WARNING: In road " << lane->GetRoad()->GetId() << " lane " << lane->GetId() <<
|
|
" no \"<width>\" parameter found under \"<lane>\" tag. Using default values." << std::endl;
|
|
}
|
|
|
|
// Lane Border
|
|
for (pugi::xml_node lane_border_node : lane_node.children("border")) {
|
|
const double s_offset = lane_border_node.attribute("sOffset").as_double();
|
|
const double a = lane_border_node.attribute("a").as_double();
|
|
const double b = lane_border_node.attribute("b").as_double();
|
|
const double c = lane_border_node.attribute("c").as_double();
|
|
const double d = lane_border_node.attribute("d").as_double();
|
|
|
|
// Call Map builder create Lane Border function
|
|
map_builder.CreateLaneBorder(lane, s_offset + s, a, b, c, d);
|
|
}
|
|
|
|
// Lane Road Mark
|
|
int road_mark_id = 0;
|
|
for (pugi::xml_node lane_road_mark : lane_node.children("roadMark")) {
|
|
pugi::xml_node road_mark_type;
|
|
{
|
|
const double s_offset = lane_road_mark.attribute("sOffset").as_double();
|
|
const std::string type = lane_road_mark.attribute("type").value();
|
|
const std::string weight = lane_road_mark.attribute("weight").value();
|
|
const std::string color = lane_road_mark.attribute("color").value();
|
|
const std::string material = lane_road_mark.attribute("material").value();
|
|
const double width = lane_road_mark.attribute("width").as_double();
|
|
const std::string lane_change = lane_road_mark.attribute("laneChange").value();
|
|
const double height = lane_road_mark.attribute("height").as_double();
|
|
|
|
// Call map builder for LaneRoadMarkType
|
|
|
|
std::string type_name = "";
|
|
double type_width = 0.0;
|
|
road_mark_type = lane_road_mark.child("type");
|
|
if (road_mark_type) {
|
|
type_name = road_mark_type.attribute("name").value();
|
|
type_width = road_mark_type.attribute("width").as_double();
|
|
}
|
|
|
|
// Call map builder for LaneRoadMark
|
|
map_builder.CreateRoadMark(
|
|
lane,
|
|
road_mark_id,
|
|
s_offset + s,
|
|
type,
|
|
weight,
|
|
color,
|
|
material,
|
|
width,
|
|
lane_change,
|
|
height,
|
|
type_name,
|
|
type_width);
|
|
}
|
|
|
|
for (pugi::xml_node road_mark_type_line_node : road_mark_type.children("line")) {
|
|
|
|
const double length = road_mark_type_line_node.attribute("length").as_double();
|
|
const double space = road_mark_type_line_node.attribute("space").as_double();
|
|
const double t = road_mark_type_line_node.attribute("tOffset").as_double();
|
|
const double s_offset = road_mark_type_line_node.attribute("sOffset").as_double();
|
|
const std::string rule = road_mark_type_line_node.attribute("rule").value();
|
|
const double width = road_mark_type_line_node.attribute("width").as_double();
|
|
|
|
// Call map builder for LaneRoadMarkType LaneRoadMarkTypeLine
|
|
map_builder.CreateRoadMarkTypeLine(
|
|
lane,
|
|
road_mark_id,
|
|
length,
|
|
space,
|
|
t,
|
|
s_offset + s,
|
|
rule,
|
|
width);
|
|
}
|
|
++road_mark_id;
|
|
}
|
|
|
|
// Lane Material
|
|
for (pugi::xml_node lane_material_node : lane_node.children("material")) {
|
|
|
|
const double s_offset = lane_material_node.attribute("sOffset").as_double();
|
|
const std::string surface = lane_material_node.attribute("surface").value();
|
|
const double friction = lane_material_node.attribute("friction").as_double();
|
|
const double roughness = lane_material_node.attribute("roughness").as_double();
|
|
|
|
// Create map builder for Lane Material
|
|
map_builder.CreateLaneMaterial(lane, s_offset + s, surface, friction, roughness);
|
|
}
|
|
|
|
// Lane Visibility
|
|
for (pugi::xml_node lane_visibility_node : lane_node.children("visibility")) {
|
|
const double s_offset = lane_visibility_node.attribute("sOffset").as_double();
|
|
const double forward = lane_visibility_node.attribute("forward").as_double();
|
|
const double back = lane_visibility_node.attribute("back").as_double();
|
|
const double left = lane_visibility_node.attribute("left").as_double();
|
|
const double right = lane_visibility_node.attribute("right").as_double();
|
|
|
|
// Create map builder for Lane Visibility
|
|
map_builder.CreateLaneVisibility(lane, s_offset + s, forward, back, left, right);
|
|
}
|
|
|
|
// Lane Speed
|
|
for (pugi::xml_node lane_speed_node : lane_node.children("speed")) {
|
|
const double s_offset = lane_speed_node.attribute("sOffset").as_double();
|
|
const double max = lane_speed_node.attribute("max").as_double();
|
|
std::string unit = lane_speed_node.attribute("unit").value();
|
|
|
|
// Create map builder for Lane Speed
|
|
map_builder.CreateLaneSpeed(lane, s_offset + s, max, unit);
|
|
}
|
|
|
|
// Lane Access
|
|
for (pugi::xml_node lane_access_node : lane_node.children("access")) {
|
|
const double s_offset = lane_access_node.attribute("sOffset").as_double();
|
|
const std::string restriction = lane_access_node.attribute("restriction").value();
|
|
|
|
// Create map builder for Lane Access
|
|
map_builder.CreateLaneAccess(lane, s_offset + s, restriction);
|
|
}
|
|
|
|
// Lane Height
|
|
for (pugi::xml_node lane_height_node : lane_node.children("height")) {
|
|
const double s_offset = lane_height_node.attribute("sOffset").as_double();
|
|
const double inner = lane_height_node.attribute("inner").as_double();
|
|
const double outer = lane_height_node.attribute("outer").as_double();
|
|
|
|
// Create map builder for Lane Height
|
|
map_builder.CreateLaneHeight(lane, s_offset + s, inner, outer);
|
|
}
|
|
|
|
// Lane Rule
|
|
for (pugi::xml_node lane_rule_node : lane_node.children("rule")) {
|
|
const double s_offset = lane_rule_node.attribute("sOffset").as_double();
|
|
const std::string value = lane_rule_node.attribute("value").value();
|
|
|
|
// Create map builder for Lane Height
|
|
map_builder.CreateLaneRule(lane, s_offset + s, value);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
void LaneParser::Parse(
|
|
const pugi::xml_document &xml,
|
|
carla::road::MapBuilder &map_builder) {
|
|
|
|
pugi::xml_node open_drive_node = xml.child("OpenDRIVE");
|
|
|
|
// Lanes
|
|
for (pugi::xml_node road_node : open_drive_node.children("road")) {
|
|
road::RoadId road_id = road_node.attribute("id").as_uint();
|
|
|
|
for (pugi::xml_node lanes_node : road_node.children("lanes")) {
|
|
|
|
for (pugi::xml_node lane_section_node : lanes_node.children("laneSection")) {
|
|
double s = lane_section_node.attribute("s").as_double();
|
|
pugi::xml_node left_node = lane_section_node.child("left");
|
|
if (left_node) {
|
|
ParseLanes(road_id, s, left_node, map_builder);
|
|
}
|
|
|
|
pugi::xml_node center_node = lane_section_node.child("center");
|
|
if (center_node) {
|
|
ParseLanes(road_id, s, center_node, map_builder);
|
|
}
|
|
|
|
pugi::xml_node right_node = lane_section_node.child("right");
|
|
if (right_node) {
|
|
ParseLanes(road_id, s, right_node, map_builder);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} // namespace parser
|
|
} // namespace opendrive
|
|
} // namespace carla
|