p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Restructured code

This commit is contained in:
Marc Garcia Puig 2019-03-12 19:05:16 +01:00 committed by nsubiron
parent e9d22073c7
commit 5949f2b9e2
36 changed files with 609 additions and 2178 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
LibCarla/source/carla/client/Map.cpp
View File

@ -7,7 +7,7 @@
#include "carla/client/Map.h"
#include "carla/client/Waypoint.h"
#include "carla/opendrive/OpenDrive.h"
#include "carla/opendrive/OpenDriveParser.h"
#include "carla/road/Map.h"
#include "carla/road/WaypointGenerator.h"
@ -18,7 +18,7 @@ namespace client {
static auto MakeMap(const std::string &opendrive_contents) {
auto stream = std::istringstream(opendrive_contents);
return opendrive::OpenDrive::Load(stream);
return opendrive::OpenDriveParser::Load(stream.str());
}
Map::Map(rpc::MapInfo description)

474
LibCarla/source/carla/opendrive/OpenDrive.cpp
View File

@ -1,474 +0,0 @@
// 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 "OpenDrive.h"
#include "carla/road/MapBuilder.h"
#include "carla/Debug.h"
namespace carla {
namespace opendrive {
static void fnc_generate_roads_data(
opendrive::types::OpenDriveData &openDriveRoad,
std::map<int, opendrive::types::RoadInformation *> &out_roads) {
for (size_t i = 0; i < openDriveRoad.roads.size(); ++i) {
out_roads[openDriveRoad.roads[i].attributes.id] = &openDriveRoad.roads[i];
}
}
static void fnc_generate_junctions_data(
opendrive::types::OpenDriveData &openDriveRoad,
std::map<int, std::map<int, std::vector<carla::road::lane_junction_t>>> &out_data) {
for (size_t i = 0; i < openDriveRoad.junctions.size(); ++i) {
for (size_t j = 0; j < openDriveRoad.junctions[i].connections.size(); ++j) {
carla::road::lane_junction_t junctionData;
const int junctionID = openDriveRoad.junctions[i].attributes.id;
const int incommingRoad = openDriveRoad.junctions[i].connections[j].attributes.incoming_road;
const int connectingRoad = openDriveRoad.junctions[i].connections[j].attributes.connecting_road;
junctionData.incomming_road = incommingRoad;
junctionData.connection_road = connectingRoad;
junctionData.contact_point = openDriveRoad.junctions[i].connections[j].attributes.contact_point;
for (size_t k = 0; k < openDriveRoad.junctions[i].connections[j].links.size(); ++k) {
junctionData.from_lane.emplace_back(openDriveRoad.junctions[i].connections[j].links[k].from);
junctionData.to_lane.emplace_back(openDriveRoad.junctions[i].connections[j].links[k].to);
}
out_data[junctionID][incommingRoad].emplace_back(junctionData);
}
}
}
// HACK(Andrei):
static int fnc_get_first_driving_line(opendrive::types::RoadInformation *roadInfo, int id = 0) {
if (roadInfo == nullptr) {
return 0;
}
for (auto itSec = roadInfo->lanes.lane_sections.rbegin();
itSec != roadInfo->lanes.lane_sections.rend() - 1;
++itSec) {
for (auto itLane = itSec->left.begin(); itLane != itSec->left.end(); ++itLane) {
if (itLane->attributes.type == "driving" && itLane->attributes.id == id) {
id = itLane->link ? itLane->link->predecessor_id : 0;
break;
}
}
}
return id;
}
SharedPtr<road::Map> OpenDrive::Load(
const std::string &file,
XmlInputType inputType,
std::string *out_error) {
carla::opendrive::types::OpenDriveData open_drive_road;
OpenDriveParser::Parse(file.c_str(), open_drive_road, inputType, out_error);
carla::road::MapBuilder mapBuilder;
if (open_drive_road.roads.empty()) {
// TODO(Andrei): Log some type of warning
return mapBuilder.Build();
}
mapBuilder.SetGeoReference(open_drive_road.geoReference);
mapBuilder.SetTrafficGroupData(open_drive_road.trafficlightgroups);
mapBuilder.SetTrafficSignData(open_drive_road.trafficsigns);
// Generate road and junction information
using road_data_t = std::map<int, carla::opendrive::types::RoadInformation *>;
using junction_data_t = std::map<int, std::map<int, std::vector<carla::road::lane_junction_t>>>;
road_data_t road_data;
junction_data_t junctions_data;
// generates a map<road_id, opendrive::types::RoadInformation>
fnc_generate_roads_data(open_drive_road, road_data);
// generates a map<junction_id, opendrive::types::RoadInformation>
fnc_generate_junctions_data(open_drive_road, junctions_data);
// Transforma data for the MapBuilder
for (road_data_t::iterator it = road_data.begin(); it != road_data.end(); ++it) {
carla::road::element::RoadSegmentDefinition road_segment(it->first);
carla::road::element::RoadInfoLane *RoadInfoLanes =
road_segment.MakeInfo<carla::road::element::RoadInfoLane>();
carla::road::element::RoadGeneralInfo *RoadGeneralInfo =
road_segment.MakeInfo<carla::road::element::RoadGeneralInfo>();
RoadGeneralInfo->SetJunctionId(it->second->attributes.junction);
for (auto &&lane_offset : it->second->lanes.lane_offset) {
// @todo: delete this old method
RoadGeneralInfo->SetLanesOffset(lane_offset.s, lane_offset.a);
// new method
road_segment.MakeInfo<carla::road::element::RoadInfoLaneOffset>(
lane_offset.s,
lane_offset.a,
lane_offset.b,
lane_offset.c,
lane_offset.d
);
}
for (auto &&elevation : it->second->road_profiles.elevation_profile) {
road_segment.MakeInfo<carla::road::element::RoadElevationInfo>(
elevation.start_position,
elevation.start_position,
elevation.elevation,
elevation.slope,
elevation.vertical_curvature,
elevation.curvature_change);
}
std::unordered_map<int, int> left_lanes_go_to_successor, left_lanes_go_to_predecessor;
// std::unordered_map<int, int> center_lanes_go_to_successor, center_lanes_go_to_predecessor;
std::unordered_map<int, int> right_lanes_go_to_successor, right_lanes_go_to_predecessor;
for (auto &&left_lanes : it->second->lanes.lane_sections[0].left) {
if (left_lanes.link != nullptr) {
if (left_lanes.attributes.type != "driving") {
continue;
}
if (left_lanes.link->successor_id != 0) {
left_lanes_go_to_successor[left_lanes.attributes.id] = left_lanes.link->successor_id;
}
left_lanes_go_to_predecessor[left_lanes.attributes.id] = left_lanes.link->predecessor_id;
if (left_lanes.link->predecessor_id != 0) {
}
}
}
// for (auto &&center_lanes : it->second->lanes.lane_sections[0].center) {
// if (center_lanes.link != nullptr) {
// if (center_lanes.link->successor_id != 0) {
// center_lanes_go_to_successor[center_lanes.attributes.id] = center_lanes.link->successor_id;
// }
// if (center_lanes.link->predecessor_id != 0) {
// center_lanes_go_to_predecessor[center_lanes.attributes.id] = center_lanes.link->predecessor_id;
// }
// }
// }
for (auto &&right_lanes : it->second->lanes.lane_sections[0].right) {
if (right_lanes.link != nullptr) {
if (right_lanes.attributes.type != "driving") {
continue;
}
if (right_lanes.link->successor_id != 0) {
right_lanes_go_to_successor[right_lanes.attributes.id] = right_lanes.link->successor_id;
}
if (right_lanes.link->predecessor_id != 0) {
right_lanes_go_to_predecessor[right_lanes.attributes.id] = right_lanes.link->predecessor_id;
}
}
}
if (it->second->lanes.lane_sections.size() > 1) {
for (auto itLaneSec = it->second->lanes.lane_sections.begin() + 1;
itLaneSec != it->second->lanes.lane_sections.end();
++itLaneSec) {
for (auto &&itLanes : itLaneSec->left) {
for (auto &&itTest : left_lanes_go_to_successor) {
if (itTest.second == itLanes.attributes.id && itLanes.link) {
itTest.second = itLanes.link->successor_id;
}
}
}
}
// for (auto itLaneSec = it->second->lanes.lane_sections.begin() + 1;
// itLaneSec != it->second->lanes.lane_sections.end();
// ++itLaneSec) {
// for (auto &&itLanes : itLaneSec->center) {
// for (auto &&itTest : center_lanes_go_to_successor) {
// if (itTest.second == itLanes.attributes.id && itLanes.link) {
// itTest.second = itLanes.link->successor_id;
// }
// }
// }
// }
for (auto itLaneSec = it->second->lanes.lane_sections.begin() + 1;
itLaneSec != it->second->lanes.lane_sections.end();
++itLaneSec) {
for (auto &&itLanes : itLaneSec->right) {
for (auto &&itTest : right_lanes_go_to_successor) {
if (itTest.second == itLanes.attributes.id && itLanes.link) {
itTest.second = itLanes.link->successor_id;
}
}
}
}
}
/////////////////////////////////////////////////////////////////////////
std::vector<carla::opendrive::types::LaneInfo> *lanesLeft =
&it->second->lanes.lane_sections.front().left;
std::vector<carla::opendrive::types::LaneInfo> *lanesRight =
&it->second->lanes.lane_sections.front().right;
for (size_t i = 0; i < lanesLeft->size(); ++i) {
RoadInfoLanes->addLaneInfo(lanesLeft->at(i).attributes.id,
lanesLeft->at(i).lane_width[0].width,
lanesLeft->at(i).attributes.type);
}
for (size_t i = 0; i < lanesRight->size(); ++i) {
RoadInfoLanes->addLaneInfo(lanesRight->at(i).attributes.id,
lanesRight->at(i).lane_width[0].width,
lanesRight->at(i).attributes.type);
}
if (it->second->road_link.successor != nullptr) {
if (it->second->road_link.successor->element_type == "junction") {
std::vector<carla::road::lane_junction_t> &options =
junctions_data[it->second->road_link.successor->id][it->first];
for (size_t i = 0; i < options.size(); ++i) {
road_segment.AddSuccessorID(options[i].connection_road, options[i].contact_point == "start");
for (size_t j = 0; j < options[i].from_lane.size(); ++j) {
bool is_end = options[i].contact_point == "end";
int to_lane = options[i].to_lane[j];
if (is_end) {
to_lane = fnc_get_first_driving_line(road_data[options[i].connection_road], to_lane);
}
road_segment.AddNextLaneInfo(options[i].from_lane[j], to_lane, options[i].connection_road);
}
}
} else {
bool is_start = it->second->road_link.successor->contact_point == "start";
road_segment.AddSuccessorID(it->second->road_link.successor->id, is_start);
for (auto &&lanes : right_lanes_go_to_successor) {
road_segment.AddNextLaneInfo(lanes.first, lanes.second, it->second->road_link.successor->id);
}
for (auto &&lanes : left_lanes_go_to_successor) {
road_segment.AddNextLaneInfo(lanes.first, lanes.second, it->second->road_link.successor->id);
}
}
}
if (it->second->road_link.predecessor != nullptr) {
if (it->second->road_link.predecessor->element_type == "junction") {
std::vector<carla::road::lane_junction_t> &options =
junctions_data[it->second->road_link.predecessor->id][it->first];
for (size_t i = 0; i < options.size(); ++i) {
road_segment.AddPredecessorID(options[i].connection_road, options[i].contact_point == "start");
for (size_t j = 0; j < options[i].from_lane.size(); ++j) {
bool is_end = options[i].contact_point == "end";
int to_lane = options[i].to_lane[j];
if (is_end) {
to_lane = fnc_get_first_driving_line(road_data[options[i].connection_road], to_lane);
}
road_segment.AddPrevLaneInfo(options[i].from_lane[j], to_lane, options[i].connection_road);
}
}
} else {
bool is_start = it->second->road_link.predecessor->contact_point == "start";
road_segment.AddPredecessorID(it->second->road_link.predecessor->id, is_start);
for (auto &&lanes : right_lanes_go_to_predecessor) {
road_segment.AddPrevLaneInfo(lanes.first, lanes.second, it->second->road_link.predecessor->id);
}
for (auto &&lanes : left_lanes_go_to_predecessor) {
road_segment.AddPrevLaneInfo(lanes.first, lanes.second, it->second->road_link.predecessor->id);
}
}
}
// Add lane change information
for (auto &&lane_section : it->second->lanes.lane_sections) {
// Lambda to convert from string to enum LaneChange
auto string_2_lane_change_enum = [](auto lane_change)->
carla::road::element::RoadInfoMarkRecord::LaneChange {
if (lane_change == "increase") {
return carla::road::element::RoadInfoMarkRecord::LaneChange::Increase;
}
else if (lane_change == "decrease") {
return carla::road::element::RoadInfoMarkRecord::LaneChange::Decrease;
}
else if (lane_change == "both") {
return carla::road::element::RoadInfoMarkRecord::LaneChange::Both;
}
else {
return carla::road::element::RoadInfoMarkRecord::LaneChange::None;
}
};
const auto lane_sec_s_offset = lane_section.start_position;
// Create a new RoadInfoMarkRecord for each lane section. Each RoadInfoMarkRecord
// will contain the actual distance from the start of the RoadSegment
for (auto &&lane_section_left : lane_section.left) {
// parse all left lane road marks
for (auto &&road_marker : lane_section_left.road_marker) {
road_segment.MakeInfo<carla::road::element::RoadInfoMarkRecord>(
lane_sec_s_offset + road_marker.soffset,
lane_section_left.attributes.id,
road_marker.type,
road_marker.weigth,
road_marker.color,
road_marker.material,
road_marker.width,
string_2_lane_change_enum(road_marker.lane_change),
// @todo: "carla/opendrive/types.h" must be extended to parse the height
0.0);
}
// parse all left lane width
for (auto &&lane_width : lane_section_left.lane_width) {
road_segment.MakeInfo<carla::road::element::RoadInfoLaneWidth>(
lane_sec_s_offset + lane_width.soffset, // s
lane_section_left.attributes.id, // lane_id
lane_width.width, // a
lane_width.slope, // b
lane_width.vertical_curvature, // c
lane_width.curvature_change); // d
}
}
// @todo: there must be only one central lane, so maybe use [0]
for (auto &&lane_section_center : lane_section.center) {
// parse all center lane road marks
for (auto &&road_marker : lane_section_center.road_marker) {
road_segment.MakeInfo<carla::road::element::RoadInfoMarkRecord>(
lane_sec_s_offset + road_marker.soffset,
lane_section_center.attributes.id,
road_marker.type,
road_marker.weigth,
road_marker.color,
road_marker.material,
road_marker.width,
string_2_lane_change_enum(road_marker.lane_change),
// @todo: "carla/opendrive/types.h" must be extended to parse the height
0.0);
}
// parse all center lane width
for (auto &&lane_width : lane_section_center.lane_width) {
road_segment.MakeInfo<carla::road::element::RoadInfoLaneWidth>(
lane_sec_s_offset + lane_width.soffset, // s
lane_section_center.attributes.id, // lane_id
lane_width.width, // a
lane_width.slope, // b
lane_width.vertical_curvature, // c
lane_width.curvature_change); // d
}
}
for (auto &&lane_section_right : lane_section.right) {
// parse all right lane road marks
for (auto &&road_marker : lane_section_right.road_marker) {
road_segment.MakeInfo<carla::road::element::RoadInfoMarkRecord>(
lane_sec_s_offset + road_marker.soffset,
lane_section_right.attributes.id,
road_marker.type,
road_marker.weigth,
road_marker.color,
road_marker.material,
road_marker.width,
string_2_lane_change_enum(road_marker.lane_change),
// @todo: "carla/opendrive/types.h" must be extended to parse the height
0.0);
}
// parse all right lane width
for (auto &&lane_width : lane_section_right.lane_width) {
road_segment.MakeInfo<carla::road::element::RoadInfoLaneWidth>(
lane_sec_s_offset + lane_width.soffset, // s
lane_section_right.attributes.id, // lane_id
lane_width.width, // a
lane_width.slope, // b
lane_width.vertical_curvature, // c
lane_width.curvature_change); // d
}
}
}
// Add geometry information
for (size_t i = 0; i < it->second->geometry_attributes.size(); ++i) {
geom::Location loc;
loc.x = it->second->geometry_attributes[i]->start_position_x;
loc.y = -it->second->geometry_attributes[i]->start_position_y;
switch (it->second->geometry_attributes[i]->type) {
case carla::opendrive::types::GeometryType::ARC: {
carla::opendrive::types::GeometryAttributesArc *arc =
(carla::opendrive::types::GeometryAttributesArc *) it->second->geometry_attributes[i].get();
road_segment.MakeGeometry<carla::road::element::GeometryArc>(arc->start_position,
arc->length,
-arc->heading,
loc,
arc->curvature);
break;
}
case carla::opendrive::types::GeometryType::LINE: {
carla::opendrive::types::GeometryAttributesLine *line =
(carla::opendrive::types::GeometryAttributesLine *) it->second->geometry_attributes[i].get();
road_segment.MakeGeometry<carla::road::element::GeometryLine>(line->start_position,
line->length,
-line->heading,
loc);
break;
}
case carla::opendrive::types::GeometryType::SPIRAL: {
carla::opendrive::types::GeometryAttributesSpiral *spiral =
(carla::opendrive::types::GeometryAttributesSpiral *) it->second->geometry_attributes[i].get();
road_segment.MakeGeometry<carla::road::element::GeometrySpiral>(spiral->start_position,
spiral->length,
-spiral->heading,
loc,
spiral->curve_start,
spiral->curve_end);
break;
}
default: {
break;
}
}
}
mapBuilder.AddRoadSegmentDefinition(road_segment);
}
return mapBuilder.Build();
}
SharedPtr<road::Map> OpenDrive::Load(std::istream &input, std::string *out_error) {
std::string fileContent;
std::string line;
while (std::getline(input, line)) {
fileContent.append(line);
}
return Load(fileContent, XmlInputType::CONTENT, out_error);
}
} // namespace opendrive
} // namespace carla

52
LibCarla/source/carla/opendrive/OpenDriveParser.cpp Normal file
View File

@ -0,0 +1,52 @@
// 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/OpenDriveParser.h"
#include "carla/Logging.h"
#include "carla/road/MapBuilder.h"
#include "carla/opendrive/parser/GeoReferenceParser.h"
#include "carla/opendrive/parser/GeometryParser.h"
#include "carla/opendrive/parser/JunctionParser.h"
#include "carla/opendrive/parser/LaneParser.h"
#include "carla/opendrive/parser/ProfilesParser.h"
#include "carla/opendrive/parser/RoadLinkParser.h"
#include "carla/opendrive/parser/TrafficGroupParser.h"
#include "carla/opendrive/parser/TrafficSignParser.h"
#include "carla/opendrive/parser/TrafficSignalsParser.h"
#include "carla/opendrive/pugixml/pugixml.hpp"
namespace carla {
namespace opendrive {
SharedPtr<road::Map> OpenDriveParser::Load(const std::string &opendrive) {
pugi::xml_document xml;
pugi::xml_parse_result parse_result = xml.load_string(opendrive.c_str());
if (parse_result == false) {
log_error("unable to parse the XML string");
return {};
}
carla::road::MapBuilder map_builder;
// GeoReferenceParser::Parse(xml, map_builder):
parser::GeometryParser::Parse(xml, map_builder);
parser::JunctionParser::Parse(xml, map_builder);
parser::LaneParser::Parse(xml, map_builder);
parser::ProfilesParser::Parse(xml, map_builder);
parser::RoadLinkParser::Parse(xml, map_builder);
parser::TrafficGroupParser::Parse(xml, map_builder);
parser::TrafficSignParser::Parse(xml, map_builder);
parser::TrafficSignalsParser::Parse(xml, map_builder);
return map_builder.Build();
}
} // namespace opendrive
} // namespace carla

15
LibCarla/source/carla/opendrive/OpenDrive.h → LibCarla/source/carla/opendrive/OpenDriveParser.h
View File

@ -8,28 +8,17 @@
#include "carla/Memory.h"
#include "carla/road/Map.h"
#include "parser/OpenDriveParser.h"
#include <istream>
#include <ostream>
#include <string>
namespace carla {
namespace opendrive {
class OpenDrive {
class OpenDriveParser {
public:
static SharedPtr<road::Map> Load(
std::istream &input,
std::string *out_error = nullptr);
static SharedPtr<road::Map> Load(const std::string &opendrive);
static SharedPtr<road::Map> Load(
const std::string &xml,
XmlInputType inputType,
std::string *out_error = nullptr);
static void Dump(const road::Map &map, std::ostream &output);
};
} // namespace opendrive

1
LibCarla/source/carla/opendrive/parser/GeoReferenceParser.h
View File

@ -18,6 +18,7 @@ namespace parser {
public:
static geom::GeoLocation Parse(const std::string &geo_reference_string);
};
} // parser

74
LibCarla/source/carla/opendrive/parser/GeometryParser.cpp
View File

@ -4,70 +4,26 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "GeometryParser.h"
#include "carla/opendrive/parser/GeometryParser.h"
#include <cassert>
#include "carla/opendrive/pugixml/pugixml.hpp"
#define ODP_ASSERT(x, ...) assert(x)
#define ODP_UNUSED(x) (void) (x)
#include "carla/road/MapBuilder.h"
void carla::opendrive::parser::GeometryParser::ParseArc(
const pugi::xml_node &xmlNode,
carla::opendrive::types::GeometryAttributesArc *out_geometry_arc) {
out_geometry_arc->type = opendrive::types::GeometryType::ARC;
out_geometry_arc->curvature = std::stod(xmlNode.attribute("curvature").value());
}
namespace carla {
namespace opendrive {
namespace parser {
void carla::opendrive::parser::GeometryParser::ParseLine(
const pugi::xml_node &xmlNode,
carla::opendrive::types::GeometryAttributesLine *out_geometry_line) {
ODP_UNUSED(xmlNode);
out_geometry_line->type = opendrive::types::GeometryType::LINE;
}
void ParseArc() {}
void carla::opendrive::parser::GeometryParser::ParseSpiral(
const pugi::xml_node &xmlNode,
carla::opendrive::types::GeometryAttributesSpiral *out_geometry_spiral) {
out_geometry_spiral->type = opendrive::types::GeometryType::SPIRAL;
out_geometry_spiral->curve_end = std::stod(xmlNode.attribute("curvEnd").value());
out_geometry_spiral->curve_start = std::stod(xmlNode.attribute("curvStart").value());
}
void ParseLine() {}
void carla::opendrive::parser::GeometryParser::Parse(
const pugi::xml_node &xmlNode,
std::vector<std::unique_ptr<carla::opendrive::types::GeometryAttributes>> &out_geometry_attributes) {
carla::opendrive::parser::GeometryParser gometry_parser;
void ParseSpiral() {}
for (pugi::xml_node roadGeometry = xmlNode.child("geometry");
roadGeometry;
roadGeometry = roadGeometry.next_sibling("geometry")) {
std::unique_ptr<opendrive::types::GeometryAttributes> geometry_attributes;
std::string firstChildName(roadGeometry.first_child().name());
void GeometryParser::Parse(
const pugi::xml_document &/* xml */,
carla::road::MapBuilder &/* map_builder */) {}
if (firstChildName == "arc") {
geometry_attributes = std::make_unique<opendrive::types::GeometryAttributesArc>();
gometry_parser.ParseArc(roadGeometry.first_child(),
static_cast<opendrive::types::GeometryAttributesArc *>(geometry_attributes.get()));
} else if (firstChildName == "line") {
geometry_attributes = std::make_unique<opendrive::types::GeometryAttributesLine>();
gometry_parser.ParseLine(roadGeometry.first_child(),
static_cast<opendrive::types::GeometryAttributesLine *>(geometry_attributes.get()));
} else if (firstChildName == "spiral") {
geometry_attributes = std::make_unique<opendrive::types::GeometryAttributesSpiral>();
gometry_parser.ParseSpiral(roadGeometry.first_child(),
static_cast<opendrive::types::GeometryAttributesSpiral *>(geometry_attributes.get()));
} else {
ODP_ASSERT(false, "Geometry type unknown");
}
geometry_attributes->start_position = std::stod(roadGeometry.attribute("s").value());
geometry_attributes->start_position_x = std::stod(roadGeometry.attribute("x").value());
geometry_attributes->start_position_y = std::stod(roadGeometry.attribute("y").value());
geometry_attributes->heading = std::stod(roadGeometry.attribute("hdg").value());
geometry_attributes->length = std::stod(roadGeometry.attribute("length").value());
out_geometry_attributes.emplace_back(std::move(geometry_attributes));
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

36
LibCarla/source/carla/opendrive/parser/GeometryParser.h
View File

@ -6,36 +6,28 @@
#pragma once
#include "../types.h"
#include "./pugixml/pugixml.hpp"
namespace pugi {
class xml_document;
} // namespace pugi
namespace carla {
namespace road {
class MapBuilder;
} // namespace road
namespace opendrive {
namespace parser {
class GeometryParser {
private:
void ParseArc(
const pugi::xml_node &xmlNode,
carla::opendrive::types::GeometryAttributesArc *out_geometry_arc);
void ParseLine(
const pugi::xml_node &xmlNode,
carla::opendrive::types::GeometryAttributesLine *out_geometry_line);
void ParseSpiral(
const pugi::xml_node &xmlNode,
carla::opendrive::types::GeometryAttributesSpiral *out_geometry_spiral);
public:
static void Parse(
const pugi::xml_node &xmlNode,
std::vector<std::unique_ptr<carla::opendrive::types::GeometryAttributes>> &out_geometry_attributes);
const pugi::xml_document &xml,
carla::road::MapBuilder &map_builder);
};
}
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

56
LibCarla/source/carla/opendrive/parser/JunctionParser.cpp
View File

@ -4,52 +4,20 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "JunctionParser.h"
#include "carla/opendrive/parser/JunctionParser.h"
void carla::opendrive::parser::JunctionParser::Parse(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::Junction> &out_junction) {
carla::opendrive::parser::JunctionParser parser;
carla::opendrive::types::Junction junction;
#include "carla/opendrive/pugixml/pugixml.hpp"
junction.attributes.id = std::atoi(xmlNode.attribute("id").value());
junction.attributes.name = xmlNode.attribute("name").value();
#include "carla/road/MapBuilder.h"
parser.ParseConnection(xmlNode, junction.connections);
out_junction.emplace_back(junction);
}
namespace carla {
namespace opendrive {
namespace parser {
void carla::opendrive::parser::JunctionParser::ParseConnection(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::JunctionConnection> &out_connections) {
for (pugi::xml_node junctionConnection = xmlNode.child("connection");
junctionConnection;
junctionConnection = junctionConnection.next_sibling("connection")) {
carla::opendrive::types::JunctionConnection jConnection;
void JunctionParser::Parse(
const pugi::xml_document &/* xml */,
carla::road::MapBuilder &/* map_builder */) {}
jConnection.attributes.id = std::atoi(junctionConnection.attribute("id").value());
jConnection.attributes.contact_point = junctionConnection.attribute("contactPoint").value();
jConnection.attributes.incoming_road = std::atoi(junctionConnection.attribute("incomingRoad").value());
jConnection.attributes.connecting_road =
std::atoi(junctionConnection.attribute("connectingRoad").value());
ParseLaneLink(junctionConnection, jConnection.links);
out_connections.emplace_back(jConnection);
}
}
void carla::opendrive::parser::JunctionParser::ParseLaneLink(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::JunctionLaneLink> &out_lane_link) {
for (pugi::xml_node junctionLaneLink = xmlNode.child("laneLink");
junctionLaneLink;
junctionLaneLink = junctionLaneLink.next_sibling("laneLink")) {
carla::opendrive::types::JunctionLaneLink jLaneLink;
jLaneLink.from = std::atoi(junctionLaneLink.attribute("from").value());
jLaneLink.to = std::atoi(junctionLaneLink.attribute("to").value());
out_lane_link.emplace_back(jLaneLink);
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

32
LibCarla/source/carla/opendrive/parser/JunctionParser.h
View File

@ -6,32 +6,28 @@
#pragma once
#include "../types.h"
#include "./pugixml/pugixml.hpp"
namespace pugi {
class xml_document;
} // namespace pugi
namespace carla {
namespace road {
class MapBuilder;
} // namespace road
namespace opendrive {
namespace parser {
class JunctionParser {
private:
void ParseConnection(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::JunctionConnection> &out_connections);
void ParseLaneLink(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::JunctionLaneLink> &out_lane_link);
public:
static void Parse(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::Junction> &out_junction);
const pugi::xml_document &xml,
carla::road::MapBuilder &map_builder);
};
}
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

167
LibCarla/source/carla/opendrive/parser/LaneParser.cpp
View File

@ -4,167 +4,20 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "LaneParser.h"
#include "carla/opendrive/parser/LaneParser.h"
#include "carla/opendrive/pugixml/pugixml.hpp"
#include "carla/road/MapBuilder.h"
namespace carla {
namespace opendrive {
namespace parser {
void LaneParser::ParseLane(
const pugi::xml_node &xmlNode,
std::vector<types::LaneInfo> &out_lane) {
for (pugi::xml_node lane = xmlNode.child("lane"); lane; lane = lane.next_sibling("lane")) {
types::LaneInfo currentLane;
currentLane.attributes.type = lane.attribute("type").value();
currentLane.attributes.level = lane.attribute("level").value();
currentLane.attributes.id = std::atoi(lane.attribute("id").value());
ParseLaneSpeed(lane, currentLane.lane_speed);
ParseLaneWidth(lane, currentLane.lane_width);
ParseLaneLink(lane.child("link"), currentLane.link);
ParseLaneRoadMark(lane, currentLane.road_marker);
out_lane.emplace_back(std::move(currentLane));
}
}
void LaneParser::ParseLaneWidth(
const pugi::xml_node &xmlNode,
std::vector<types::LaneWidth> &out_lane_width) {
for (pugi::xml_node laneWidth = xmlNode.child("width");
laneWidth;
laneWidth = laneWidth.next_sibling("width")) {
types::LaneWidth laneWidthInfo;
laneWidthInfo.soffset = std::stod(laneWidth.attribute("sOffset").value());
laneWidthInfo.width = std::stod(laneWidth.attribute("a").value());
laneWidthInfo.slope = std::stod(laneWidth.attribute("b").value());
laneWidthInfo.vertical_curvature = std::stod(laneWidth.attribute("c").value());
laneWidthInfo.curvature_change = std::stod(laneWidth.attribute("d").value());
out_lane_width.emplace_back(laneWidthInfo);
}
}
void LaneParser::ParseLaneLink(
const pugi::xml_node &xmlNode,
std::unique_ptr<types::LaneLink> &out_lane_link) {
const pugi::xml_node predecessorNode = xmlNode.child("predecessor");
const pugi::xml_node successorNode = xmlNode.child("successor");
out_lane_link =
(predecessorNode || successorNode) ? std::make_unique<opendrive::types::LaneLink>() : nullptr;
if (out_lane_link == nullptr) {
return;
}
out_lane_link->predecessor_id = predecessorNode ? std::atoi(predecessorNode.attribute("id").value()) : 0;
out_lane_link->successor_id = successorNode ? std::atoi(successorNode.attribute("id").value()) : 0;
}
void LaneParser::ParseLaneOffset(
const pugi::xml_node &xmlNode,
std::vector<types::LaneOffset> &out_lane_offset) {
types::LaneOffset lanesOffset;
lanesOffset.s = std::stod(xmlNode.attribute("s").value());
lanesOffset.a = std::stod(xmlNode.attribute("a").value());
lanesOffset.b = std::stod(xmlNode.attribute("b").value());
lanesOffset.c = std::stod(xmlNode.attribute("c").value());
lanesOffset.d = std::stod(xmlNode.attribute("d").value());
out_lane_offset.emplace_back(lanesOffset);
}
void LaneParser::ParseLaneRoadMark(
const pugi::xml_node &xmlNode,
std::vector<types::LaneRoadMark> &out_lane_mark) {
for (pugi::xml_node road_mark = xmlNode.child("roadMark");
road_mark; road_mark = road_mark.next_sibling("roadMark")) {
types::LaneRoadMark roadMarker;
if (road_mark.attribute("sOffset") != nullptr) {
roadMarker.soffset = std::stod(road_mark.attribute("sOffset").value());
}
if (road_mark.attribute("width") != nullptr) {
roadMarker.width = std::stod(road_mark.attribute("width").value());
}
if (road_mark.attribute("type") != nullptr) {
roadMarker.type = road_mark.attribute("type").value();
}
if (road_mark.attribute("weight") != nullptr) {
roadMarker.weigth = road_mark.attribute("weight").value();
}
if (road_mark.attribute("material") != nullptr) {
roadMarker.color = road_mark.attribute("material").value();
}
if (road_mark.attribute("color") != nullptr) {
roadMarker.color = road_mark.attribute("color").value();
}
if (road_mark.attribute("laneChange") != nullptr) {
roadMarker.lane_change = road_mark.attribute("laneChange").value();
}
out_lane_mark.emplace_back(roadMarker);
}
}
void LaneParser::ParseLaneSpeed(
const pugi::xml_node &xmlNode,
std::vector<types::LaneSpeed> &out_lane_speed) {
for (pugi::xml_node laneSpeed = xmlNode.child("speed");
laneSpeed;
laneSpeed = laneSpeed.next_sibling("speed")) {
types::LaneSpeed lane_speed = { 0.0, 0.0 };
lane_speed.soffset = std::stod(laneSpeed.attribute("sOffset").value());
lane_speed.max_speed = std::stod(laneSpeed.attribute("max").value());
out_lane_speed.emplace_back(lane_speed);
}
}
void LaneParser::Parse(
const pugi::xml_node &xmlNode,
types::Lanes &out_lanes) {
LaneParser laneParser;
const pugi::xml_document &/* xml */,
carla::road::MapBuilder &/* map_builder */) {}
for (pugi::xml_node laneSection = xmlNode.child("laneOffset");
laneSection;
laneSection = laneSection.next_sibling("laneOffset")) {
laneParser.ParseLaneOffset(laneSection, out_lanes.lane_offset);
}
for (pugi::xml_node laneSection = xmlNode.child("laneSection");
laneSection;
laneSection = laneSection.next_sibling("laneSection")) {
types::LaneSection laneSec;
laneSec.start_position = std::stod(laneSection.attribute("s").value());
pugi::xml_node lane = laneSection.child("left");
laneParser.ParseLane(lane, laneSec.left);
lane = laneSection.child("center");
laneParser.ParseLane(lane, laneSec.center);
lane = laneSection.child("right");
laneParser.ParseLane(lane, laneSec.right);
out_lanes.lane_sections.emplace_back(std::move(laneSec));
}
}
} // parser
} // opendrive
} // carla
} // namespace parser
} // namespace opendrive
} // namespace carla

48
LibCarla/source/carla/opendrive/parser/LaneParser.h
View File

@ -6,48 +6,28 @@
#pragma once
#include "../types.h"
#include "./pugixml/pugixml.hpp"
namespace pugi {
class xml_document;
} // namespace pugi
namespace carla {
namespace road {
class MapBuilder;
} // namespace road
namespace opendrive {
namespace parser {
class LaneParser {
private:
void ParseLane(
const pugi::xml_node &xmlNode,
std::vector<types::LaneInfo> &out_lane);
void ParseLaneSpeed(
const pugi::xml_node &xmlNode,
std::vector<types::LaneSpeed> &out_lane_speed);
void ParseLaneLink(
const pugi::xml_node &xmlNode,
std::unique_ptr<types::LaneLink> &out_lane_link);
void ParseLaneOffset(
const pugi::xml_node &xmlNode,
std::vector<types::LaneOffset> &out_lane_offset);
void ParseLaneWidth(
const pugi::xml_node &xmlNode,
std::vector<types::LaneWidth> &out_lane_width);
void ParseLaneRoadMark(
const pugi::xml_node &xmlNode,
std::vector<types::LaneRoadMark> &out_lane_mark);
public:
static void Parse(
const pugi::xml_node &xmlNode,
types::Lanes &out_lanes);
const pugi::xml_document &xml,
carla::road::MapBuilder &map_builder);
};
} // parser
} // opendrive
} // carla
} // namespace parser
} // namespace opendrive
} // namespace carla

102
LibCarla/source/carla/opendrive/parser/OpenDriveParser.cpp
View File

@ -1,102 +0,0 @@
// 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/OpenDriveParser.h"
#include "carla/geom/GeoLocation.h"
#include "carla/opendrive/parser/GeoReferenceParser.h"
#include "carla/opendrive/parser/GeometryParser.h"
#include "carla/opendrive/parser/JunctionParser.h"
#include "carla/opendrive/parser/LaneParser.h"
#include "carla/opendrive/parser/ProfilesParser.h"
#include "carla/opendrive/parser/RoadLinkParser.h"
#include "carla/opendrive/parser/TrafficGroupParser.h"
#include "carla/opendrive/parser/TrafficSignParser.h"
#include "carla/opendrive/parser/TrafficSignalsParser.h"
#include "./pugixml/pugixml.hpp"
bool OpenDriveParser::Parse(
const char *xml,
carla::opendrive::types::OpenDriveData &out_open_drive_data,
XmlInputType inputType,
std::string *out_error) {
namespace odp = carla::opendrive::parser;
pugi::xml_document xmlDoc;
pugi::xml_parse_result pugiParseResult;
switch (inputType) {
case XmlInputType::FILE: {
pugiParseResult = xmlDoc.load_file(xml);
} break;
case XmlInputType::CONTENT: {
pugiParseResult = xmlDoc.load_string(xml);
} break;
default: {
// TODO(Andrei): Log some kind of error
return false;
} break;
}
if (pugiParseResult == false) {
if (out_error != nullptr) {
*out_error = pugiParseResult.description();
}
return false;
}
for (pugi::xml_node road = xmlDoc.child("OpenDRIVE").child("road");
road;
road = road.next_sibling("road")) {
carla::opendrive::types::RoadInformation openDriveRoadInformation;
openDriveRoadInformation.attributes.name = road.attribute("name").value();
openDriveRoadInformation.attributes.id = std::atoi(road.attribute("id").value());
openDriveRoadInformation.attributes.length = std::stod(road.attribute("length").value());
openDriveRoadInformation.attributes.junction = std::atoi(road.attribute("junction").value());
///////////////////////////////////////////////////////////////////////////////
odp::ProfilesParser::Parse(road, openDriveRoadInformation.road_profiles);
odp::RoadLinkParser::Parse(road.child("link"), openDriveRoadInformation.road_link);
odp::TrafficSignalsParser::Parse(road.child("signals"),
openDriveRoadInformation.trafic_signals);
odp::LaneParser::Parse(road.child("lanes"), openDriveRoadInformation.lanes);
odp::GeometryParser::Parse(road.child("planView"),
openDriveRoadInformation.geometry_attributes);
out_open_drive_data.roads.emplace_back(std::move(openDriveRoadInformation));
}
for (pugi::xml_node junction = xmlDoc.child("OpenDRIVE").child("junction");
junction;
junction = junction.next_sibling("junction")) {
odp::JunctionParser::Parse(junction, out_open_drive_data.junctions);
}
for (pugi::xml_node tlgroup = xmlDoc.child("OpenDRIVE").child("tlGroup");
tlgroup;
tlgroup = tlgroup.next_sibling("tlGroup")) {
odp::TrafficGroupParser::Parse(tlgroup, out_open_drive_data.trafficlightgroups);
}
for (pugi::xml_node trafficsigns = xmlDoc.child("OpenDRIVE").child("trafficsign");
trafficsigns;
trafficsigns = trafficsigns.next_sibling("trafficsign")) {
odp::TrafficSignParser::Parse(trafficsigns, out_open_drive_data.trafficsigns);
}
out_open_drive_data.geoReference = odp::GeoReferenceParser::Parse(
xmlDoc.child("OpenDRIVE").child("header").child_value("geoReference"));
return true;
}

22
LibCarla/source/carla/opendrive/parser/OpenDriveParser.h
View File

@ -1,22 +0,0 @@
// 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>.
#pragma once
#include "carla/opendrive/types.h"
enum class XmlInputType : int {
FILE,
CONTENT
};
struct OpenDriveParser {
static bool Parse(
const char *xml,
carla::opendrive::types::OpenDriveData &out_open_drive_data,
XmlInputType inputType,
std::string *out_error = nullptr);
};

53
LibCarla/source/carla/opendrive/parser/ProfilesParser.cpp
View File

@ -4,49 +4,20 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "ProfilesParser.h"
#include "carla/opendrive/parser/ProfilesParser.h"
void carla::opendrive::parser::ProfilesParser::ParseElevation(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::ElevationProfile> &out_elevation_profile) {
for (pugi::xml_node laneSection = xmlNode.child("elevation");
laneSection;
laneSection = laneSection.next_sibling("elevation")) {
carla::opendrive::types::ElevationProfile elevationProfile;
#include "carla/opendrive/pugixml/pugixml.hpp"
elevationProfile.start_position = std::stod(laneSection.attribute("s").value());
elevationProfile.elevation = std::stod(laneSection.attribute("a").value());
elevationProfile.slope = std::stod(laneSection.attribute("b").value());
elevationProfile.vertical_curvature = std::stod(laneSection.attribute("c").value());
elevationProfile.curvature_change = std::stod(laneSection.attribute("d").value());
#include "carla/road/MapBuilder.h"
out_elevation_profile.emplace_back(elevationProfile);
}
}
namespace carla {
namespace opendrive {
namespace parser {
void carla::opendrive::parser::ProfilesParser::ParseLateral(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::LateralProfile> &out_lateral_profile) {
for (pugi::xml_node laneSection = xmlNode.child("superelevation");
laneSection;
laneSection = laneSection.next_sibling("superelevation")) {
carla::opendrive::types::LateralProfile lateralProfile;
void ProfilesParser::Parse(
const pugi::xml_document &/* xml */,
carla::road::MapBuilder &/* map_builder */) {}
lateralProfile.start_position = std::stod(laneSection.attribute("s").value());
lateralProfile.elevation = std::stod(laneSection.attribute("a").value());
lateralProfile.slope = std::stod(laneSection.attribute("b").value());
lateralProfile.vertical_curvature = std::stod(laneSection.attribute("c").value());
lateralProfile.curvature_change = std::stod(laneSection.attribute("d").value());
out_lateral_profile.emplace_back(lateralProfile);
}
}
void carla::opendrive::parser::ProfilesParser::Parse(
const pugi::xml_node &xmlNode,
carla::opendrive::types::RoadProfiles &out_road_profiles) {
carla::opendrive::parser::ProfilesParser profilesParser;
profilesParser.ParseElevation(xmlNode.child("elevationProfile"), out_road_profiles.elevation_profile);
profilesParser.ParseLateral(xmlNode.child("lateralProfile"), out_road_profiles.lateral_profile);
}
} // namespace parser
} // namespace opendrive
} // namespace carla

32
LibCarla/source/carla/opendrive/parser/ProfilesParser.h
View File

@ -6,32 +6,28 @@
#pragma once
#include "../types.h"
#include "./pugixml/pugixml.hpp"
namespace pugi {
class xml_document;
} // namespace pugi
namespace carla {
namespace road {
class MapBuilder;
} // namespace road
namespace opendrive {
namespace parser {
class ProfilesParser {
private:
void ParseElevation(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::ElevationProfile> &out_elevation_profile);
void ParseLateral(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::LateralProfile> &out_lateral_profile);
public:
static void Parse(
const pugi::xml_node &xmlNode,
carla::opendrive::types::RoadProfiles &out_road_profiles);
const pugi::xml_document &xml,
carla::road::MapBuilder &map_builder);
};
}
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

37
LibCarla/source/carla/opendrive/parser/RoadLinkParser.cpp
View File

@ -4,33 +4,20 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "RoadLinkParser.h"
#include "carla/opendrive/parser/RoadLinkParser.h"
#include <cstdlib>
#include "carla/opendrive/pugixml/pugixml.hpp"
void carla::opendrive::parser::RoadLinkParser::ParseLink(
const pugi::xml_node &xmlNode,
carla::opendrive::types::RoadLinkInformation *out_link_information) {
out_link_information->id = std::atoi(xmlNode.attribute("elementId").value());
out_link_information->element_type = xmlNode.attribute("elementType").value();
out_link_information->contact_point = xmlNode.attribute("contactPoint").value();
}
#include "carla/road/MapBuilder.h"
void carla::opendrive::parser::RoadLinkParser::Parse(
const pugi::xml_node &xmlNode,
carla::opendrive::types::RoadLink &out_road_link) {
RoadLinkParser parser;
namespace carla {
namespace opendrive {
namespace parser {
const pugi::xml_node predecessorNode = xmlNode.child("predecessor");
const pugi::xml_node successorNode = xmlNode.child("successor");
void RoadLinkParser::Parse(
const pugi::xml_document &/* xml */,
carla::road::MapBuilder &/* map_builder */) {}
if (predecessorNode) {
out_road_link.predecessor = std::make_unique<carla::opendrive::types::RoadLinkInformation>();
parser.ParseLink(predecessorNode, out_road_link.predecessor.get());
}
if (successorNode) {
out_road_link.successor = std::make_unique<carla::opendrive::types::RoadLinkInformation>();
parser.ParseLink(successorNode, out_road_link.successor.get());
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

28
LibCarla/source/carla/opendrive/parser/RoadLinkParser.h
View File

@ -6,28 +6,28 @@
#pragma once
#include "../types.h"
#include "./pugixml/pugixml.hpp"
namespace pugi {
class xml_document;
} // namespace pugi
namespace carla {
namespace road {
class MapBuilder;
} // namespace road
namespace opendrive {
namespace parser {
class RoadLinkParser {
private:
void ParseLink(
const pugi::xml_node &xmlNode,
carla::opendrive::types::RoadLinkInformation *out_link_information);
public:
static void Parse(
const pugi::xml_node &xmlNode,
carla::opendrive::types::RoadLink &out_road_link);
const pugi::xml_document &xml,
carla::road::MapBuilder &map_builder);
};
}
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

66
LibCarla/source/carla/opendrive/parser/TrafficGroupParser.cpp
View File

@ -4,62 +4,20 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "TrafficGroupParser.h"
#include <iostream>
#include "carla/opendrive/parser/TrafficGroupParser.h"
void carla::opendrive::parser::TrafficGroupParser::Parse(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::TrafficLightGroup> &out_trafficLights) {
carla::opendrive::parser::TrafficGroupParser parser;
carla::opendrive::types::TrafficLightGroup traffic_light_group;
#include "carla/opendrive/pugixml/pugixml.hpp"
traffic_light_group.red_time = std::atoi(xmlNode.attribute("redTime").value());
traffic_light_group.yellow_time = std::atoi(xmlNode.attribute("yellowTime").value());
traffic_light_group.green_time = std::atoi(xmlNode.attribute("greenTime").value());
#include "carla/road/MapBuilder.h"
parser.ParseTrafficLight(xmlNode, traffic_light_group.traffic_lights);
out_trafficLights.emplace_back(traffic_light_group);
}
namespace carla {
namespace opendrive {
namespace parser {
void carla::opendrive::parser::TrafficGroupParser::ParseTrafficLight(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::TrafficLight> &out_trafficLight) {
void TrafficGroupParser::Parse(
const pugi::xml_document &/* xml */,
carla::road::MapBuilder &/* map_builder */) {}
for (pugi::xml_node trafficlight = xmlNode.child("trafficlight");
trafficlight;
trafficlight = trafficlight.next_sibling("trafficlight")) {
carla::opendrive::types::TrafficLight jTrafficlight;
jTrafficlight.x_pos = std::stod(trafficlight.attribute("xPos").value());
jTrafficlight.y_pos = std::stod(trafficlight.attribute("yPos").value());
jTrafficlight.z_pos = std::stod(trafficlight.attribute("zPos").value());
jTrafficlight.x_rot = std::stod(trafficlight.attribute("xRot").value());
jTrafficlight.y_rot = std::stod(trafficlight.attribute("yRot").value());
jTrafficlight.z_rot = std::stod(trafficlight.attribute("zRot").value());
ParseBoxAreas(trafficlight, jTrafficlight.box_areas);
out_trafficLight.emplace_back(jTrafficlight);
}
}
void carla::opendrive::parser::TrafficGroupParser::ParseBoxAreas(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::BoxComponent> &out_boxcomponent) {
for (pugi::xml_node boxcomponent = xmlNode.child("tfBox");
boxcomponent;
boxcomponent = boxcomponent.next_sibling("tfBox")) {
carla::opendrive::types::BoxComponent jBoxComponent;
jBoxComponent.x_pos = std::stod(boxcomponent.attribute("xPos").value());
jBoxComponent.y_pos = std::stod(boxcomponent.attribute("yPos").value());
jBoxComponent.z_pos = std::stod(boxcomponent.attribute("zPos").value());
jBoxComponent.x_rot = std::stod(boxcomponent.attribute("xRot").value());
jBoxComponent.y_rot = std::stod(boxcomponent.attribute("yRot").value());
jBoxComponent.z_rot = std::stod(boxcomponent.attribute("zRot").value());
out_boxcomponent.emplace_back(jBoxComponent);
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

31
LibCarla/source/carla/opendrive/parser/TrafficGroupParser.h
View File

@ -6,33 +6,28 @@
#pragma once
#include "../types.h"
#include "./pugixml/pugixml.hpp"
namespace pugi {
class xml_document;
} // namespace pugi
namespace carla {
namespace road {
class MapBuilder;
} // namespace road
namespace opendrive {
namespace parser {
class TrafficGroupParser {
private:
void ParseTrafficLight(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::TrafficLight> &out_trafficLights);
void ParseBoxAreas(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::BoxComponent> &out_boxareas);
public:
static void Parse(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::TrafficLightGroup> &out_trafficlightgroup);
const pugi::xml_document &xml,
carla::road::MapBuilder &map_builder);
};
}
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

47
LibCarla/source/carla/opendrive/parser/TrafficSignParser.cpp
View File

@ -4,43 +4,20 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "TrafficSignParser.h"
#include <iostream>
#include "carla/opendrive/parser/TrafficSignParser.h"
void carla::opendrive::parser::TrafficSignParser::Parse(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::TrafficSign> &out_trafficsigns) {
carla::opendrive::parser::TrafficSignParser parser;
carla::opendrive::types::TrafficSign trafficsign;
#include "carla/opendrive/pugixml/pugixml.hpp"
trafficsign.speed = std::atoi(xmlNode.attribute("speed").value());
trafficsign.x_pos = std::stod(xmlNode.attribute("xPos").value());
trafficsign.y_pos = std::stod(xmlNode.attribute("yPos").value());
trafficsign.z_pos = std::stod(xmlNode.attribute("zPos").value());
trafficsign.x_rot = std::stod(xmlNode.attribute("xRot").value());
trafficsign.y_rot = std::stod(xmlNode.attribute("yRot").value());
trafficsign.z_rot = std::stod(xmlNode.attribute("zRot").value());
#include "carla/road/MapBuilder.h"
parser.ParseBoxAreas(xmlNode, trafficsign.box_areas);
out_trafficsigns.emplace_back(trafficsign);
}
namespace carla {
namespace opendrive {
namespace parser {
void carla::opendrive::parser::TrafficSignParser::ParseBoxAreas(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::BoxComponent> &out_boxcomponent) {
for (pugi::xml_node boxcomponent = xmlNode.child("tsBox");
boxcomponent;
boxcomponent = boxcomponent.next_sibling("tsBox")) {
carla::opendrive::types::BoxComponent jBoxComponent;
void TrafficSignParser::Parse(
const pugi::xml_document &/* xml */,
carla::road::MapBuilder &/* map_builder */) {}
jBoxComponent.x_pos = std::stod(boxcomponent.attribute("xPos").value());
jBoxComponent.y_pos = std::stod(boxcomponent.attribute("yPos").value());
jBoxComponent.z_pos = std::stod(boxcomponent.attribute("zPos").value());
jBoxComponent.x_rot = std::stod(boxcomponent.attribute("xRot").value());
jBoxComponent.y_rot = std::stod(boxcomponent.attribute("yRot").value());
jBoxComponent.z_rot = std::stod(boxcomponent.attribute("zRot").value());
out_boxcomponent.emplace_back(jBoxComponent);
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

27
LibCarla/source/carla/opendrive/parser/TrafficSignParser.h
View File

@ -6,29 +6,28 @@
#pragma once
#include "../types.h"
#include "./pugixml/pugixml.hpp"
namespace pugi {
class xml_document;
} // namespace pugi
namespace carla {
namespace road {
class MapBuilder;
} // namespace road
namespace opendrive {
namespace parser {
class TrafficSignParser {
private:
void ParseBoxAreas(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::BoxComponent> &out_boxareas);
public:
static void Parse(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::TrafficSign> &out_trafficsigns);
const pugi::xml_document &xml,
carla::road::MapBuilder &map_builder);
};
}
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

34
LibCarla/source/carla/opendrive/parser/TrafficSignalsParser.cpp
View File

@ -4,30 +4,20 @@
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
#include "TrafficSignalsParser.h"
#include "carla/opendrive/parser/TrafficSignalsParser.h"
void carla::opendrive::parser::TrafficSignalsParser::Parse(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::TrafficSignalInformation> &out_traffic_signals) {
for (pugi::xml_node signal = xmlNode.child("signal"); signal; signal = signal.next_sibling("signal")) {
carla::opendrive::types::TrafficSignalInformation trafficSignalInformation;
#include "carla/opendrive/pugixml/pugixml.hpp"
trafficSignalInformation.id = std::atoi(signal.attribute("id").value());
#include "carla/road/MapBuilder.h"
trafficSignalInformation.start_position = std::stod(signal.attribute("s").value());
trafficSignalInformation.track_position = std::stod(signal.attribute("t").value());
namespace carla {
namespace opendrive {
namespace parser {
trafficSignalInformation.zoffset = std::stod(signal.attribute("zOffset").value());
trafficSignalInformation.value = std::stod(signal.attribute("value").value());
void TrafficSignalsParser::Parse(
const pugi::xml_document &/* xml */,
carla::road::MapBuilder &/* map_builder */) {}
trafficSignalInformation.name = signal.attribute("name").value();
trafficSignalInformation.dynamic = signal.attribute("dynamic").value();
trafficSignalInformation.orientation = signal.attribute("orientation").value();
trafficSignalInformation.type = signal.attribute("type").value();
trafficSignalInformation.subtype = signal.attribute("subtype").value();
trafficSignalInformation.country = signal.attribute("country").value();
out_traffic_signals.emplace_back(trafficSignalInformation);
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

22
LibCarla/source/carla/opendrive/parser/TrafficSignalsParser.h
View File

@ -6,11 +6,16 @@
#pragma once
#include "../types.h"
#include "./pugixml/pugixml.hpp"
namespace pugi {
class xml_document;
} // namespace pugi
namespace carla {
namespace road {
class MapBuilder;
} // namespace road
namespace opendrive {
namespace parser {
@ -18,10 +23,11 @@ namespace parser {
public:
static void Parse(
const pugi::xml_node &xmlNode,
std::vector<carla::opendrive::types::TrafficSignalInformation> &out_traffic_signals);
const pugi::xml_document &xml,
carla::road::MapBuilder &map_builder);
};
}
}
}
} // namespace parser
} // namespace opendrive
} // namespace carla

0
LibCarla/source/carla/opendrive/parser/pugixml/LICENSE.md → LibCarla/source/carla/opendrive/pugixml/LICENSE.md
View File

0
LibCarla/source/carla/opendrive/parser/pugixml/pugiconfig.hpp → LibCarla/source/carla/opendrive/pugixml/pugiconfig.hpp
View File

0
LibCarla/source/carla/opendrive/parser/pugixml/pugixml.cpp → LibCarla/source/carla/opendrive/pugixml/pugixml.cpp
View File

0
LibCarla/source/carla/opendrive/parser/pugixml/pugixml.hpp → LibCarla/source/carla/opendrive/pugixml/pugixml.hpp
View File

335
LibCarla/source/carla/opendrive/types.h
View File

@ -1,335 +0,0 @@
// 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>.
#pragma once
#include "carla/geom/GeoLocation.h"
#include <memory>
#include <string>
#include <vector>
namespace carla {
namespace opendrive {
namespace types {
enum class GeometryType : unsigned int {
ARC,
LINE,
SPIRAL
};
struct GeometryAttributes {
GeometryType type; // geometry type
double length; // length of the road section
// [meters]
double start_position; // s-offset [meters]
double heading; // start orientation [radians]
double start_position_x; // [meters]
double start_position_y; // [meters]
};
struct GeometryAttributesArc : public GeometryAttributes {
double curvature;
};
struct GeometryAttributesLine : public GeometryAttributes {
// Nothing else here
};
struct GeometryAttributesSpiral : public GeometryAttributes {
double curve_start;
double curve_end;
};
/////////////////////////////////////////////////////////////////
struct LaneAttributes {
int id;
std::string type;
std::string level;
};
struct LaneWidth {
double soffset; // start position (s-offset)
// [meters]
double width; // a - width [meters]
double slope; // b
double vertical_curvature; // c
double curvature_change; // d
};
struct LaneRoadMark {
double soffset = 0.0;
double width = 0.0;
std::string type;
std::string weigth = "standard";
// See OpenDRIVE Format Specification, Rev. 1.4
// Doc No.: VI2014.107 (5.3.7.2.1.1.4 Road Mark Record)
std::string material = "standard";
std::string color = "white";
std::string lane_change = "none";
};
struct LaneOffset {
double s, a, b, c, d;
};
struct LaneSpeed {
double soffset; // start position(s - offset from the
// current lane section) [meters]
double max_speed; // maximum allowed speed [meters/second]
};
struct LaneLink {
int predecessor_id;
int successor_id;
};
struct LaneInfo {
std::vector<LaneSpeed> lane_speed;
LaneAttributes attributes;
std::vector<LaneWidth> lane_width;
std::vector<LaneRoadMark> road_marker;
std::unique_ptr<LaneLink> link;
};
struct LaneSection {
double start_position;
std::vector<LaneInfo> left, center, right;
};
struct Lanes {
std::vector<LaneOffset> lane_offset;
std::vector<LaneSection> lane_sections;
};
/////////////////////////////////////////////////////////////////
struct ElevationProfile {
double start_position; // (S) start position(s -
// offset)[meters]
double elevation; // (A) elevation [meters]
double slope; // (B)
double vertical_curvature; // (C)
double curvature_change; // (D)
};
struct LateralProfile {
double start_position; // (S) start position(s -
// offset)[meters]
double elevation; // (A) elevation [meters]
double slope; // (B)
double vertical_curvature; // (C)
double curvature_change; // (D)
};
struct RoadProfiles {
std::vector<ElevationProfile> elevation_profile;
std::vector<LateralProfile> lateral_profile;
};
/////////////////////////////////////////////////////////////////
struct TrafficSignalInformation {
int id;
double start_position; // s
double track_position; // t
double zoffset; // z offset from track level
double value; // value of the signal (e.g. speed,
// mass <20> depending on type)
std::string name; // name of the signal (e.g. gfx bead
// name)
std::string dynamic; // boolean identification whether
// signal is a dynamic
// signal(e.g.traffic light)
std::string orientation; // "+" = valid in positive track
// direction; "-" = valid in
// negative track direction; "none"
// = valid in both directions
std::string country; // country code of the signa
std::string type; // type identifier according to
// country code or "-1" / "none"
std::string subtype; // subtype identifier according to
// country code or "-1" / "none"
};
/////////////////////////////////////////////////////////////////
struct RoadAttributes {
std::string name;
int id, junction;
double length;
RoadAttributes() : id(-1),
junction(-1),
length(0.0) {}
};
struct RoadLinkInformation {
int id;
std::string element_type;
std::string contact_point;
RoadLinkInformation() : id(-1) {}
};
struct RoadLink {
std::unique_ptr<RoadLinkInformation> successor;
std::unique_ptr<RoadLinkInformation> predecessor;
};
struct RoadInformation {
RoadLink road_link;
RoadProfiles road_profiles;
RoadAttributes attributes;
Lanes lanes;
std::vector<TrafficSignalInformation> trafic_signals;
std::vector<std::unique_ptr<GeometryAttributes>> geometry_attributes;
};
/////////////////////////////////////////////////////////////////
struct JunctionAttribues {
int id;
std::string name;
JunctionAttribues() : id(-1) {}
};
struct JunctionConnectionAttributes {
int id;
int incoming_road;
int connecting_road;
std::string contact_point;
JunctionConnectionAttributes() : id(-1),
incoming_road(-1),
connecting_road(-1) {}
};
struct JunctionLaneLink {
int from;
int to;
JunctionLaneLink() : from(-1),
to(-1) {}
};
struct JunctionConnection {
JunctionConnectionAttributes attributes;
std::vector<JunctionLaneLink> links;
};
struct Junction {
JunctionAttribues attributes;
std::vector<JunctionConnection> connections;
};
struct BoxComponent {
union {
struct { double x_pos, y_pos, z_pos;
};
double pos[3];
};
union {
struct { double x_rot, y_rot, z_rot;
};
double rot[3];
};
double scale;
BoxComponent() : pos{0.0, 0.0, 0.0},
rot{0.0, 0.0, 0.0},
scale(1.0) {}
};
struct TrafficLight {
union {
struct { double x_pos, y_pos, z_pos;
};
double pos[3];
};
union {
struct { double x_rot, y_rot, z_rot;
};
double rot[3];
};
double scale;
std::vector<BoxComponent> box_areas;
TrafficLight() : pos{0.0, 0.0, 0.0},
rot{0.0, 0.0, 0.0},
scale(1.0) {}
};
struct TrafficLightGroup {
std::vector<TrafficLight> traffic_lights;
double red_time, yellow_time, green_time;
};
struct TrafficSign {
union {
struct { double x_pos, y_pos, z_pos;
};
double pos[3];
};
union {
struct { double x_rot, y_rot, z_rot;
};
double rot[3];
};
double scale;
int speed;
std::vector<BoxComponent> box_areas;
TrafficSign() : pos{0.0, 0.0, 0.0},
rot{0.0, 0.0, 0.0},
scale(1.0),
speed(30) {}
};
/////////////////////////////////////////////////////////////////
struct OpenDriveData {
geom::GeoLocation geoReference;
std::vector<RoadInformation> roads;
std::vector<Junction> junctions;
std::vector<TrafficLightGroup> trafficlightgroups;
std::vector<TrafficSign> trafficsigns;
};
struct Waypoint {
union {
struct { double x, y, z;
};
double v[3];
};
double heading, speed, width;
double distance_in_section;
Waypoint() : v{0.0, 0.0, 0.0},
heading(0.0),
speed(0.0),
width(0.0) {}
};
}
}
}

27
LibCarla/source/carla/road/Map.cpp
View File

@ -6,6 +6,9 @@
#include "carla/road/Map.h"
#include <stdexcept>
#include "carla/Exception.h"
#include "carla/road/element/LaneCrossingCalculator.h"
namespace carla {
@ -17,22 +20,24 @@ namespace road {
return Waypoint(shared_from_this(), loc);
}
boost::optional<Waypoint> Map::GetWaypoint(const geom::Location &loc) const {
Waypoint w = Waypoint(shared_from_this(), loc);
auto d = geom::Math::Distance2D(w.ComputeTransform().location, loc);
const auto inf = _data.GetRoad(w._road_id)->GetInfo<RoadInfoLane>(w._dist);
if (d < inf->getLane(w._lane_id)->_width * 0.5) {
return w;
}
boost::optional<Waypoint> Map::GetWaypoint(const geom::Location &/* loc */) const {
// Waypoint w = Waypoint(shared_from_this(), loc);
// auto d = geom::Math::Distance2D(w.ComputeTransform().location, loc);
// const auto inf = _data.GetRoad(w._road_id)->GetInfo<RoadInfoLane>(w._dist);
// if (d < inf->getLane(w._lane_id)->_width * 0.5) {
// return w;
// }
throw_exception(std::runtime_error("not implemented"));
return {};
}
std::vector<element::LaneMarking> Map::CalculateCrossedLanes(
const geom::Location &origin,
const geom::Location &destination) const {
return element::LaneCrossingCalculator::Calculate(*this, origin, destination);
const geom::Location &/* origin */,
const geom::Location &/* destination */) const {
// return element::LaneCrossingCalculator::Calculate(*this, origin, destination);
throw_exception(std::runtime_error("not implemented"));
return {};
}
const MapData &Map::GetData() const {

77
LibCarla/source/carla/road/MapBuilder.cpp
View File

@ -12,94 +12,17 @@ using namespace carla::road::element;
namespace carla {
namespace road {
bool MapBuilder::AddRoadSegmentDefinition(RoadSegmentDefinition &seg) {
_temp_sections.emplace(seg.GetId(), std::move(seg));
return true;
}
SharedPtr<Map> MapBuilder::Build() {
// Move the RoadSegmentDefinitions needed information to a RoadSegments
for (auto &&id_seg : _temp_sections) {
MakeElement<RoadSegment>(id_seg.first, std::move(id_seg.second));
}
SetTotalRoadSegmentLength();
CreatePointersBetweenRoadSegments();
ComputeLaneCenterOffset();
// _map_data is a memeber of MapBuilder so you must especify if
// you want to keep it (will return copy -> Map(const Map &))
// or move it (will return move -> Map(Map &&))
return SharedPtr<Map>(new Map{std::move(_map_data)});
}
void MapBuilder::SetTotalRoadSegmentLength() {
for (auto &&id_seg : _map_data._elements) {
double total_length = 0.0;
for (auto &&geom : id_seg.second.get()->_geom) {
total_length += geom.get()->GetLength();
}
id_seg.second.get()->_length = total_length;
}
}
void MapBuilder::CreatePointersBetweenRoadSegments() {
for (auto &&id_seg : _temp_sections) {
for (auto &t : id_seg.second.GetPredecessorID()) {
_map_data._elements[id_seg.first]->PredEmplaceBack(_map_data._elements[t].get());
}
for (auto &t : id_seg.second.GetSuccessorID()) {
_map_data._elements[id_seg.first]->SuccEmplaceBack(_map_data._elements[t].get());
}
}
}
void MapBuilder::ComputeLaneCenterOffset() {
for (auto &&element : _map_data._elements) {
RoadSegment *road_seg = element.second.get();
// get all the RoadGeneralInfo given the type and a distance 0.0
auto up_bound_g = decltype(road_seg->_info)::reverse_iterator(road_seg->_info.upper_bound(0.0));
auto general_info = MakeRoadInfoIterator<RoadGeneralInfo>(up_bound_g, road_seg->_info.rend());
// get all the RoadInfoLane given the type and a distance 0.0
auto up_bound_l = decltype(road_seg->_info)::reverse_iterator(road_seg->_info.upper_bound(0.0));
auto lane_info = MakeRoadInfoIterator<RoadInfoLane>(up_bound_l, road_seg->_info.rend());
// check that have a RoadGeneralInfo
if (!lane_info.IsAtEnd()) {
double lane_offset = 0.0;
if (!general_info.IsAtEnd()) {
lane_offset = (*general_info)->GetLanesOffset().at(0).second;
}
double current_width = lane_offset;
for (auto &&current_lane_id :
(*lane_info)->getLanesIDs(element::RoadInfoLane::which_lane_e::Left)) {
const double half_width = (*lane_info)->getLane(current_lane_id)->_width * 0.5;
current_width += half_width;
(*lane_info)->_lanes[current_lane_id]._lane_center_offset = current_width;
current_width += half_width;
}
current_width = lane_offset;
for (auto &&current_lane_id :
(*lane_info)->getLanesIDs(element::RoadInfoLane::which_lane_e::Right)) {
const double half_width = (*lane_info)->getLane(current_lane_id)->_width * 0.5;
current_width -= half_width;
(*lane_info)->_lanes[current_lane_id]._lane_center_offset = current_width;
current_width -= half_width;
}
}
}
}
} // namespace road
} // namespace carla

32
LibCarla/source/carla/road/MapBuilder.h
View File

@ -17,49 +17,17 @@ namespace road {
class MapBuilder {
public:
bool AddRoadSegmentDefinition(element::RoadSegmentDefinition &seg);
void SetJunctionInformation(const std::vector<carla::road::lane_junction_t> &junctionInfo) {
_map_data.SetJunctionInformation(junctionInfo);
}
void SetGeoReference(const geom::GeoLocation &geoReference) {
_map_data.SetGeoReference(geoReference);
}
void SetTrafficGroupData(const std::vector<opendrive::types::TrafficLightGroup> &trafficLightData) {
_map_data.SetTrafficLightData(trafficLightData);
}
void SetTrafficSignData(const std::vector<opendrive::types::TrafficSign> &trafficSignData) {
_map_data.SetTrafficSignData(trafficSignData);
}
SharedPtr<Map> Build();
private:
template <typename T, typename ... Args>
T &MakeElement(element::id_type id, Args && ... args) {
auto inst = std::make_unique<T>(std::forward<Args>(args) ...);
T &r = *inst;
_map_data._elements.emplace(id, std::move(inst));
return r;
}
/// Set the total length of each road based on the geometries
void SetTotalRoadSegmentLength();
/// Create the pointers between RoadSegments based on the ids
void CreatePointersBetweenRoadSegments();
/// Set the _lane_center_offset of all the lanes
void ComputeLaneCenterOffset();
private:
MapData _map_data;
std::map<element::id_type, element::RoadSegmentDefinition> _temp_sections;
};
} // namespace road

76
LibCarla/source/carla/road/MapData.h
View File

@ -10,7 +10,7 @@
#include "carla/ListView.h"
#include "carla/NonCopyable.h"
#include "carla/road/element/RoadSegment.h"
#include "carla/opendrive/types.h"
#include "carla/geom/GeoLocation.h"
#include <boost/iterator/transform_iterator.hpp>
@ -19,93 +19,21 @@
namespace carla {
namespace road {
struct lane_junction_t {
std::string contact_point = "start";
int junction_id = -1;
int connection_road = -1;
int incomming_road = -1;
std::vector<int> from_lane;
std::vector<int> to_lane;
};
class MapData
: private MovableNonCopyable {
class MapData : private MovableNonCopyable {
public:
const element::RoadSegment *GetRoad(element::id_type id) const {
auto it = _elements.find(id);
return it != _elements.end() ? it->second.get() : nullptr;
}
auto GetAllIds() const {
return MakeListView(
iterator::make_map_keys_iterator(_elements.begin()),
iterator::make_map_keys_iterator(_elements.end()));
}
size_t GetRoadCount() const {
return _elements.size();
}
const std::vector<lane_junction_t> &GetJunctionInformation() const {
return _junction_information;
}
const geom::GeoLocation &GetGeoReference() const {
return _geo_reference;
}
const std::vector<opendrive::types::TrafficLightGroup> &GetTrafficGroups() const {
return _traffic_groups;
}
const std::vector<opendrive::types::TrafficSign> &GetTrafficSigns() const {
return _traffic_signs;
}
auto GetRoadSegments() const {
using const_ref = const element::RoadSegment &;
auto get = [](auto &pair) -> const_ref { return *pair.second; };
return MakeListView(
boost::make_transform_iterator(_elements.begin(), get),
boost::make_transform_iterator(_elements.end(), get));
}
private:
friend class MapBuilder;
MapData() = default;
void SetJunctionInformation(const std::vector<lane_junction_t> &junctionInfo) {
_junction_information = junctionInfo;
}
void SetGeoReference(const geom::GeoLocation &geoReference) {
_geo_reference = geoReference;
}
void SetTrafficLightData(const std::vector<opendrive::types::TrafficLightGroup> &trafficLightData) {
_traffic_groups = trafficLightData;
}
void SetTrafficSignData(const std::vector<opendrive::types::TrafficSign> &trafficSignData) {
_traffic_signs = trafficSignData;
}
geom::GeoLocation _geo_reference;
std::vector<lane_junction_t> _junction_information;
std::unordered_map<
element::id_type,
std::unique_ptr<element::RoadSegment>> _elements;
std::vector<opendrive::types::TrafficLightGroup> _traffic_groups;
std::vector<opendrive::types::TrafficSign> _traffic_signs;
};
} // namespace road

347
LibCarla/source/carla/road/WaypointGenerator.cpp
View File

@ -6,6 +6,9 @@
#include "carla/road/WaypointGenerator.h"
#include <stdexcept>
#include "carla/Exception.h"
#include "carla/road/Map.h"
namespace carla {
@ -18,210 +21,228 @@ namespace road {
// ===========================================================================
template <typename T>
static std::vector<T> ConcatVectors(std::vector<T> dst, std::vector<T> src) {
if (src.size() > dst.size()) {
return ConcatVectors(src, dst);
}
dst.insert(
dst.end(),
std::make_move_iterator(src.begin()),
std::make_move_iterator(src.end()));
return dst;
static std::vector<T> ConcatVectors(std::vector<T> /* dst */, std::vector<T> /* src */) {
// if (src.size() > dst.size()) {
// return ConcatVectors(src, dst);
// }
// dst.insert(
// dst.end(),
// std::make_move_iterator(src.begin()),
// std::make_move_iterator(src.end()));
// return dst;
return {};
}
template <typename FuncT>
static void ForEachDrivableLane(const RoadSegment &road, double s, FuncT &&func) {
const auto info = road.GetInfo<RoadInfoLane>(s);
DEBUG_ASSERT(info != nullptr);
for (auto &&lane_id : info->getLanesIDs(RoadInfoLane::which_lane_e::Both)) {
if (info->getLane(lane_id)->_type == "driving") {
func(lane_id);
}
}
static void ForEachDrivableLane(const RoadSegment &/* road */, double /* s */, FuncT &&/* func */) {
throw_exception(std::runtime_error("not implemented"));
// const auto info = road.GetInfo<RoadInfoLane>(s);
// DEBUG_ASSERT(info != nullptr);
// for (auto &&lane_id : info->getLanesIDs(RoadInfoLane::which_lane_e::Both)) {
// if (info->getLane(lane_id)->_type == "driving") {
// func(lane_id);
// }
// }
}
// ===========================================================================
// -- WaypointGenerator ------------------------------------------------------
// ===========================================================================
std::vector<Waypoint> WaypointGenerator::GetSuccessors(const Waypoint &waypoint) {
auto &map = waypoint._map;
const auto this_lane_id = waypoint.GetLaneId();
const auto this_road_id = waypoint.GetRoadId();
std::vector<Waypoint> WaypointGenerator::GetSuccessors(const Waypoint &/* waypoint */) {
// auto &map = waypoint._map;
// const auto this_lane_id = waypoint.GetLaneId();
// const auto this_road_id = waypoint.GetRoadId();
const auto &next_lanes =
this_lane_id <= 0 ?
waypoint.GetRoadSegment().GetNextLane(this_lane_id) :
waypoint.GetRoadSegment().GetPrevLane(this_lane_id);
// const auto &next_lanes =
// this_lane_id <= 0 ?
// waypoint.GetRoadSegment().GetNextLane(this_lane_id) :
// waypoint.GetRoadSegment().GetPrevLane(this_lane_id);
if (next_lanes.empty()) {
log_error("road id =", this_road_id, "lane id =", this_lane_id, ": missing next lanes");
}
// if (next_lanes.empty()) {
// log_error("road id =", this_road_id, "lane id =", this_lane_id, ": missing next lanes");
// }
std::vector<Waypoint> result;
result.reserve(next_lanes.size());
for (auto &&pair : next_lanes) {
const auto lane_id = pair.first;
const auto road_id = pair.second;
const auto road = map->GetData().GetRoad(road_id);
DEBUG_ASSERT(lane_id != 0);
DEBUG_ASSERT(road != nullptr);
const auto distance = lane_id < 0 ? 0.0 : road->GetLength();
result.push_back(Waypoint(map, road_id, lane_id, distance));
}
return result;
// std::vector<Waypoint> result;
// result.reserve(next_lanes.size());
// for (auto &&pair : next_lanes) {
// const auto lane_id = pair.first;
// const auto road_id = pair.second;
// const auto road = map->GetData().GetRoad(road_id);
// DEBUG_ASSERT(lane_id != 0);
// DEBUG_ASSERT(road != nullptr);
// const auto distance = lane_id < 0 ? 0.0 : road->GetLength();
// result.push_back(Waypoint(map, road_id, lane_id, distance));
// }
// return result;
throw_exception(std::runtime_error("not implemented"));
return {};
}
std::vector<Waypoint> WaypointGenerator::GetNext(
const Waypoint &waypoint,
double distance) {
auto &map = waypoint._map;
const auto this_road_id = waypoint.GetRoadId();
const auto this_lane_id = waypoint.GetLaneId();
const Waypoint &/* waypoint */,
double /* distance */) {
// auto &map = waypoint._map;
// const auto this_road_id = waypoint.GetRoadId();
// const auto this_lane_id = waypoint.GetLaneId();
DEBUG_ASSERT(this_lane_id != 0);
// DEBUG_ASSERT(this_lane_id != 0);
double distance_on_next_segment;
// double distance_on_next_segment;
if (this_lane_id <= 0) {
// road goes forward.
const auto total_distance = waypoint._dist + distance;
const auto road_length = waypoint.GetRoadSegment().GetLength();
if (total_distance <= road_length) {
return { Waypoint(map, this_road_id, this_lane_id, total_distance) };
}
distance_on_next_segment = total_distance - road_length;
} else {
// road goes backward.
const auto total_distance = waypoint._dist - distance;
if (total_distance >= 0.0) {
return { Waypoint(map, this_road_id, this_lane_id, total_distance) };
}
distance_on_next_segment = std::abs(total_distance);
}
// if (this_lane_id <= 0) {
// // road goes forward.
// const auto total_distance = waypoint._dist + distance;
// const auto road_length = waypoint.GetRoadSegment().GetLength();
// if (total_distance <= road_length) {
// return { Waypoint(map, this_road_id, this_lane_id, total_distance) };
// }
// distance_on_next_segment = total_distance - road_length;
// } else {
// // road goes backward.
// const auto total_distance = waypoint._dist - distance;
// if (total_distance >= 0.0) {
// return { Waypoint(map, this_road_id, this_lane_id, total_distance) };
// }
// distance_on_next_segment = std::abs(total_distance);
// }
std::vector<Waypoint> result;
for (auto &&next_waypoint : GetSuccessors(waypoint)) {
result = ConcatVectors(result, GetNext(next_waypoint, distance_on_next_segment));
}
return result;
// std::vector<Waypoint> result;
// for (auto &&next_waypoint : GetSuccessors(waypoint)) {
// result = ConcatVectors(result, GetNext(next_waypoint, distance_on_next_segment));
// }
// return result;
throw_exception(std::runtime_error("not implemented"));
return {};
}
boost::optional<Waypoint> WaypointGenerator::GetRight(const Waypoint &waypoint) {
auto &map = waypoint._map;
const auto this_road_id = waypoint.GetRoadId();
const auto this_lane_id = waypoint.GetLaneId();
boost::optional<Waypoint> WaypointGenerator::GetRight(const Waypoint &/* waypoint */) {
// auto &map = waypoint._map;
// const auto this_road_id = waypoint.GetRoadId();
// const auto this_lane_id = waypoint.GetLaneId();
DEBUG_ASSERT(this_lane_id != 0);
// DEBUG_ASSERT(this_lane_id != 0);
const int new_lane_id = (this_lane_id <= 0) ? this_lane_id - 1 : this_lane_id + 1;
// const int new_lane_id = (this_lane_id <= 0) ? this_lane_id - 1 : this_lane_id + 1;
// check if that lane id exists on this distance
const auto road = map->GetData().GetRoad(this_road_id);
const auto mark_record_vector = road->GetRoadInfoMarkRecord(waypoint._dist);
for (auto &&mark_record : mark_record_vector) {
// find if the lane id exists
if (mark_record->GetLaneId() == new_lane_id) {
return Waypoint(map, this_road_id, new_lane_id, waypoint._dist);
}
}
return boost::optional<Waypoint>();
// // check if that lane id exists on this distance
// const auto road = map->GetData().GetRoad(this_road_id);
// const auto mark_record_vector = road->GetRoadInfoMarkRecord(waypoint._dist);
// for (auto &&mark_record : mark_record_vector) {
// // find if the lane id exists
// if (mark_record->GetLaneId() == new_lane_id) {
// return Waypoint(map, this_road_id, new_lane_id, waypoint._dist);
// }
// }
// return boost::optional<Waypoint>();
throw_exception(std::runtime_error("not implemented"));
return {};
}
boost::optional<Waypoint> WaypointGenerator::GetLeft(const Waypoint &waypoint) {
auto &map = waypoint._map;
const auto this_road_id = waypoint.GetRoadId();
const auto this_lane_id = waypoint.GetLaneId();
boost::optional<Waypoint> WaypointGenerator::GetLeft(const Waypoint &/* waypoint */) {
// auto &map = waypoint._map;
// const auto this_road_id = waypoint.GetRoadId();
// const auto this_lane_id = waypoint.GetLaneId();
DEBUG_ASSERT(this_lane_id != 0);
// DEBUG_ASSERT(this_lane_id != 0);
int new_lane_id;
if (this_lane_id > 0) {
// road goes backward: decrease the lane id while avoiding returning lane 0
new_lane_id = this_lane_id - 1 == 0 ? -1 : this_lane_id - 1;
} else {
// road goes forward: increasing the lane id while avoiding returning lane 0
new_lane_id = this_lane_id + 1 == 0 ? 1 : this_lane_id + 1;
}
// int new_lane_id;
// if (this_lane_id > 0) {
// // road goes backward: decrease the lane id while avoiding returning lane 0
// new_lane_id = this_lane_id - 1 == 0 ? -1 : this_lane_id - 1;
// } else {
// // road goes forward: increasing the lane id while avoiding returning lane 0
// new_lane_id = this_lane_id + 1 == 0 ? 1 : this_lane_id + 1;
// }
// check if that lane id exists on this distance
const auto road = map->GetData().GetRoad(this_road_id);
const auto mark_record_vector = road->GetRoadInfoMarkRecord(waypoint._dist);
for (auto &&mark_record : mark_record_vector) {
// find if the lane id exists
if (mark_record->GetLaneId() == new_lane_id) {
return Waypoint(map, this_road_id, new_lane_id, waypoint._dist);
}
}
return boost::optional<Waypoint>();
// // check if that lane id exists on this distance
// const auto road = map->GetData().GetRoad(this_road_id);
// const auto mark_record_vector = road->GetRoadInfoMarkRecord(waypoint._dist);
// for (auto &&mark_record : mark_record_vector) {
// // find if the lane id exists
// if (mark_record->GetLaneId() == new_lane_id) {
// return Waypoint(map, this_road_id, new_lane_id, waypoint._dist);
// }
// }
// return boost::optional<Waypoint>();
throw_exception(std::runtime_error("not implemented"));
return {};
}
std::vector<Waypoint> WaypointGenerator::GenerateAll(
const Map &map,
const double distance) {
std::vector<Waypoint> result;
for (auto &&road_segment : map.GetData().GetRoadSegments()) {
/// @todo Should distribute them equally along the segment?
for (double s = 0.0; s < road_segment.GetLength(); s += distance) {
ForEachDrivableLane(road_segment, s, [&](auto lane_id) {
result.push_back(Waypoint(map.shared_from_this(), road_segment.GetId(), lane_id, s));
});
}
}
return result;
const Map &/* map */,
const double /* distance */) {
// std::vector<Waypoint> result;
// for (auto &&road_segment : map.GetData().GetRoadSegments()) {
// /// @todo Should distribute them equally along the segment?
// for (double s = 0.0; s < road_segment.GetLength(); s += distance) {
// ForEachDrivableLane(road_segment, s, [&](auto lane_id) {
// result.push_back(Waypoint(map.shared_from_this(), road_segment.GetId(), lane_id, s));
// });
// }
// }
// return result;
throw_exception(std::runtime_error("not implemented"));
return {};
}
std::vector<Waypoint> WaypointGenerator::GenerateLaneBegin(
const Map &map) {
std::vector<Waypoint> result;
for (auto &&road_segment : map.GetData().GetRoadSegments()) {
ForEachDrivableLane(road_segment, 0.0, [&](auto lane_id) {
auto distance = lane_id < 0 ? 0.0 : road_segment.GetLength();
auto this_waypoint = Waypoint(
map.shared_from_this(),
road_segment.GetId(),
lane_id,
distance);
result.push_back(this_waypoint);
});
}
return result;
const Map &/* map */) {
// std::vector<Waypoint> result;
// for (auto &&road_segment : map.GetData().GetRoadSegments()) {
// ForEachDrivableLane(road_segment, 0.0, [&](auto lane_id) {
// auto distance = lane_id < 0 ? 0.0 : road_segment.GetLength();
// auto this_waypoint = Waypoint(
// map.shared_from_this(),
// road_segment.GetId(),
// lane_id,
// distance);
// result.push_back(this_waypoint);
// });
// }
// return result;
throw_exception(std::runtime_error("not implemented"));
return {};
}
std::vector<Waypoint> WaypointGenerator::GenerateLaneEnd(
const Map &map) {
std::vector<Waypoint> result;
for (auto &&road_segment : map.GetData().GetRoadSegments()) {
ForEachDrivableLane(road_segment, 0.0, [&](auto lane_id) {
auto distance = lane_id > 0 ? 0.0 : road_segment.GetLength();
auto this_waypoint = Waypoint(
map.shared_from_this(),
road_segment.GetId(),
lane_id,
distance);
result.push_back(this_waypoint);
});
}
return result;
const Map &/* map */) {
// std::vector<Waypoint> result;
// for (auto &&road_segment : map.GetData().GetRoadSegments()) {
// ForEachDrivableLane(road_segment, 0.0, [&](auto lane_id) {
// auto distance = lane_id > 0 ? 0.0 : road_segment.GetLength();
// auto this_waypoint = Waypoint(
// map.shared_from_this(),
// road_segment.GetId(),
// lane_id,
// distance);
// result.push_back(this_waypoint);
// });
// }
// return result;
throw_exception(std::runtime_error("not implemented"));
return {};
}
std::vector<std::pair<Waypoint, Waypoint>> WaypointGenerator::GenerateTopology(
const Map &map) {
std::vector<std::pair<Waypoint, Waypoint>> result;
for (auto &&road_segment : map.GetData().GetRoadSegments()) {
ForEachDrivableLane(road_segment, 0.0, [&](auto lane_id) {
auto distance = lane_id < 0 ? 0.0 : road_segment.GetLength();
auto this_waypoint = Waypoint(
map.shared_from_this(),
road_segment.GetId(),
lane_id,
distance);
for (auto &&successor : GetSuccessors(this_waypoint)) {
result.push_back({this_waypoint, successor});
}
});
}
return result;
const Map &/* map */) {
// std::vector<std::pair<Waypoint, Waypoint>> result;
// for (auto &&road_segment : map.GetData().GetRoadSegments()) {
// ForEachDrivableLane(road_segment, 0.0, [&](auto lane_id) {
// auto distance = lane_id < 0 ? 0.0 : road_segment.GetLength();
// auto this_waypoint = Waypoint(
// map.shared_from_this(),
// road_segment.GetId(),
// lane_id,
// distance);
// for (auto &&successor : GetSuccessors(this_waypoint)) {
// result.push_back({this_waypoint, successor});
// }
// });
// }
// return result;
throw_exception(std::runtime_error("not implemented"));
return {};
}
} // namespace road

270
LibCarla/source/carla/road/element/Waypoint.cpp
View File

@ -10,6 +10,9 @@
#include "carla/geom/CubicPolynomial.h"
#include "carla/geom/Math.h"
#include <stdexcept>
#include "carla/Exception.h"
#include <unordered_map>
#include <algorithm>
@ -17,64 +20,65 @@ namespace carla {
namespace road {
namespace element {
Waypoint::Waypoint(SharedPtr<const Map> m, const geom::Location &loc)
Waypoint::Waypoint(SharedPtr<const Map> m, const geom::Location &/* loc */)
: _map(m) {
DEBUG_ASSERT(_map != nullptr);
// max_nearests represents the max nearests roads
// where we will search for nearests lanes
constexpr int max_nearests = 10;
// in case that map has less than max_nearests lanes,
// we will use the maximum lanes
const int max_nearest_allowed = _map->GetData().GetRoadCount() <
max_nearests ? _map->GetData().GetRoadCount() : max_nearests;
throw_exception(std::runtime_error("not implemented"));
// DEBUG_ASSERT(_map != nullptr);
// // max_nearests represents the max nearests roads
// // where we will search for nearests lanes
// constexpr int max_nearests = 10;
// // in case that map has less than max_nearests lanes,
// // we will use the maximum lanes
// const int max_nearest_allowed = _map->GetData().GetRoadCount() <
// max_nearests ? _map->GetData().GetRoadCount() : max_nearests;
double nearest_dist[max_nearests];
std::fill(nearest_dist, nearest_dist + max_nearest_allowed,
std::numeric_limits<double>::max());
// double nearest_dist[max_nearests];
// std::fill(nearest_dist, nearest_dist + max_nearest_allowed,
// std::numeric_limits<double>::max());
id_type ids[max_nearests];
std::fill(ids, ids + max_nearest_allowed, 0);
// id_type ids[max_nearests];
// std::fill(ids, ids + max_nearest_allowed, 0);
double dists[max_nearests];
std::fill(dists, dists + max_nearest_allowed, 0.0);
// double dists[max_nearests];
// std::fill(dists, dists + max_nearest_allowed, 0.0);
for (auto &&r : _map->GetData().GetRoadSegments()) {
auto current_dist = r.GetNearestPoint(loc);
// for (auto &&r : _map->GetData().GetRoadSegments()) {
// auto current_dist = r.GetNearestPoint(loc);
// search for nearests points
for (int i = 0; i < max_nearest_allowed; ++i) {
if (current_dist.second < nearest_dist[i]) {
// reorder nearest_dist
for (int j = max_nearest_allowed - 1; j > i; --j) {
nearest_dist[j] = nearest_dist[j - 1];
ids[j] = ids[j - 1];
dists[j] = dists[j - 1];
}
nearest_dist[i] = current_dist.second;
ids[i] = r.GetId();
dists[i] = current_dist.first;
// // search for nearests points
// for (int i = 0; i < max_nearest_allowed; ++i) {
// if (current_dist.second < nearest_dist[i]) {
// // reorder nearest_dist
// for (int j = max_nearest_allowed - 1; j > i; --j) {
// nearest_dist[j] = nearest_dist[j - 1];
// ids[j] = ids[j - 1];
// dists[j] = dists[j - 1];
// }
// nearest_dist[i] = current_dist.second;
// ids[i] = r.GetId();
// dists[i] = current_dist.first;
break;
}
}
}
// break;
// }
// }
// }
// search for the nearest lane in nearest_dist
auto nearest_lane_dist = std::numeric_limits<double>::max();
for (int i = 0; i < max_nearest_allowed; ++i) {
auto lane_dist = _map->GetData().GetRoad(ids[i])->GetNearestLane(dists[i], loc);
// // search for the nearest lane in nearest_dist
// auto nearest_lane_dist = std::numeric_limits<double>::max();
// for (int i = 0; i < max_nearest_allowed; ++i) {
// auto lane_dist = _map->GetData().GetRoad(ids[i])->GetNearestLane(dists[i], loc);
if (lane_dist.second < nearest_lane_dist) {
nearest_lane_dist = lane_dist.second;
_lane_id = lane_dist.first;
_road_id = ids[i];
_dist = dists[i];
}
}
// if (lane_dist.second < nearest_lane_dist) {
// nearest_lane_dist = lane_dist.second;
// _lane_id = lane_dist.first;
// _road_id = ids[i];
// _dist = dists[i];
// }
// }
DEBUG_ASSERT(_dist <= _map->GetData().GetRoad(_road_id)->GetLength());
DEBUG_ASSERT(_lane_id != 0);
// DEBUG_ASSERT(_dist <= _map->GetData().GetRoad(_road_id)->GetLength());
// DEBUG_ASSERT(_lane_id != 0);
}
Waypoint::Waypoint(
@ -93,117 +97,119 @@ namespace element {
Waypoint::~Waypoint() = default;
geom::Transform Waypoint::ComputeTransform() const {
const auto road_segment = _map->GetData().GetRoad(_road_id);
DEBUG_ASSERT(road_segment != nullptr);
// const auto road_segment = _map->GetData().GetRoad(_road_id);
// DEBUG_ASSERT(road_segment != nullptr);
road::element::DirectedPoint dp = road_segment->GetDirectedPointIn(_dist);
// road::element::DirectedPoint dp = road_segment->GetDirectedPointIn(_dist);
geom::Rotation rot(geom::Math::to_degrees(dp.pitch), geom::Math::to_degrees(dp.tangent), 0.0);
if (_lane_id > 0) {
rot.yaw += 180.0;
rot.pitch = 360 - rot.pitch;
}
// geom::Rotation rot(geom::Math::to_degrees(dp.pitch), geom::Math::to_degrees(dp.tangent), 0.0);
// if (_lane_id > 0) {
// rot.yaw += 180.0;
// rot.pitch = 360 - rot.pitch;
// }
if (IsIntersection()) {
// @todo: fix intersection lane_id to allow compute the lane distance in a correct way
// old way to calculate lane position
const auto *road_segment = _map->GetData().GetRoad(_road_id);
DEBUG_ASSERT(road_segment != nullptr);
const auto info = road_segment->GetInfo<RoadInfoLane>(0.0);
DEBUG_ASSERT(info != nullptr);
// if (IsIntersection()) {
// // @todo: fix intersection lane_id to allow compute the lane distance in a correct way
// // old way to calculate lane position
// const auto *road_segment = _map->GetData().GetRoad(_road_id);
// DEBUG_ASSERT(road_segment != nullptr);
// const auto info = road_segment->GetInfo<RoadInfoLane>(0.0);
// DEBUG_ASSERT(info != nullptr);
dp.ApplyLateralOffset(info->getLane(_lane_id)->_lane_center_offset);
} else {
// new way to calculate lane position
const auto lane_offset_info = road_segment->GetInfo<RoadInfoLaneOffset>(_dist);
geom::CubicPolynomial final_polynomial = lane_offset_info->GetPolynomial();
// dp.ApplyLateralOffset(info->getLane(_lane_id)->_lane_center_offset);
// } else {
// // new way to calculate lane position
// const auto lane_offset_info = road_segment->GetInfo<RoadInfoLaneOffset>(_dist);
// geom::CubicPolynomial final_polynomial = lane_offset_info->GetPolynomial();
// fill a map (lane_info_map) with first lane id <id, RoadInfoLaneWidth> found
// because will be the nearest one that is affecting at this dist (t)
const auto lane_width_info = road_segment->GetInfos<RoadInfoLaneWidth>(_dist);
std::unordered_map<int, std::shared_ptr<const RoadInfoLaneWidth>> lane_info_map;
std::unordered_set<int> inserted_lanes;
// // fill a map (lane_info_map) with first lane id <id, RoadInfoLaneWidth> found
// // because will be the nearest one that is affecting at this dist (t)
// const auto lane_width_info = road_segment->GetInfos<RoadInfoLaneWidth>(_dist);
// std::unordered_map<int, std::shared_ptr<const RoadInfoLaneWidth>> lane_info_map;
// std::unordered_set<int> inserted_lanes;
for (auto &&lane_offset : lane_width_info) {
const int current_lane_id = lane_offset->GetLaneId();
lane_info_map.emplace(
std::pair<int, std::shared_ptr<const RoadInfoLaneWidth>>(
current_lane_id,
lane_offset));
}
// for (auto &&lane_offset : lane_width_info) {
// const int current_lane_id = lane_offset->GetLaneId();
// lane_info_map.emplace(
// std::pair<int, std::shared_ptr<const RoadInfoLaneWidth>>(
// current_lane_id,
// lane_offset));
// }
DEBUG_ASSERT(_lane_id != 0);
// DEBUG_ASSERT(_lane_id != 0);
// iterate over the previous lanes until lane_id is 0 and add the polynomial info
const int inc = _lane_id < 0 ? - 1 : + 1;
// increase or decrease the lane_id depending on if _lane_id is
// positive or negative in order to get closer to that id
for (int lane_id = inc; lane_id != _lane_id; lane_id += inc) {
// final_polynomial += geom::CubicPolynomial();
final_polynomial += lane_info_map[lane_id]->GetPolynomial() * (_lane_id < 0 ? -1.0 : 1.0);
}
// // iterate over the previous lanes until lane_id is 0 and add the polynomial info
// const int inc = _lane_id < 0 ? - 1 : + 1;
// // increase or decrease the lane_id depending on if _lane_id is
// // positive or negative in order to get closer to that id
// for (int lane_id = inc; lane_id != _lane_id; lane_id += inc) {
// // final_polynomial += geom::CubicPolynomial();
// final_polynomial += lane_info_map[lane_id]->GetPolynomial() * (_lane_id < 0 ? -1.0 : 1.0);
// }
// use half of the last polynomial to get the center of the road
final_polynomial += lane_info_map[_lane_id]->GetPolynomial() * (_lane_id < 0 ? -0.5 : 0.5);
// // use half of the last polynomial to get the center of the road
// final_polynomial += lane_info_map[_lane_id]->GetPolynomial() * (_lane_id < 0 ? -0.5 : 0.5);
// compute the final lane offset
dp.ApplyLateralOffset(final_polynomial.Evaluate(_dist));
// // compute the final lane offset
// dp.ApplyLateralOffset(final_polynomial.Evaluate(_dist));
const auto tangent = geom::Math::to_degrees(final_polynomial.Tangent(_dist));
rot.yaw += _lane_id < 0 ? -tangent : tangent;
}
// const auto tangent = geom::Math::to_degrees(final_polynomial.Tangent(_dist));
// rot.yaw += _lane_id < 0 ? -tangent : tangent;
// }
return geom::Transform(dp.location, rot);
// return geom::Transform(dp.location, rot);
throw_exception(std::runtime_error("not implemented"));
return {};
}
const std::string &Waypoint::GetType() const {
return _map->GetData().GetRoad(_road_id)->GetInfo<RoadInfoLane>(_dist)->getLane(_lane_id)->_type;
}
const RoadSegment &Waypoint::GetRoadSegment() const {
const auto *road_segment = _map->GetData().GetRoad(_road_id);
DEBUG_ASSERT(road_segment != nullptr);
return *road_segment;
std::string Waypoint::GetType() const {
// return _map->GetData().GetRoad(_road_id)->GetInfo<RoadInfoLane>(_dist)->getLane(_lane_id)->_type;
throw_exception(std::runtime_error("not implemented"));
return {};
}
bool Waypoint::IsIntersection() const {
const auto info = GetRoadSegment().GetInfo<RoadGeneralInfo>(_dist);
return info != nullptr ? info->IsJunction() : false;
// const auto info = GetRoadSegment().GetInfo<RoadGeneralInfo>(_dist);
// return info != nullptr ? info->IsJunction() : false;
throw_exception(std::runtime_error("not implemented"));
return {};
}
double Waypoint::GetLaneWidth() const {
const auto *road_segment = _map->GetData().GetRoad(_road_id);
const auto lane_width_info = road_segment->GetInfos<RoadInfoLaneWidth>(_dist);
for (auto &&lane : lane_width_info) {
if (lane->GetLaneId() == _lane_id) {
return lane->GetPolynomial().Evaluate(_dist);
}
}
return 0.0;
throw_exception(std::runtime_error("not implemented"));
// const auto *road_segment = _map->GetData().GetRoad(_road_id);
// const auto lane_width_info = road_segment->GetInfos<RoadInfoLaneWidth>(_dist);
// for (auto &&lane : lane_width_info) {
// if (lane->GetLaneId() == _lane_id) {
// return lane->GetPolynomial().Evaluate(_dist);
// }
// }
// return 0.0;
return {};
}
std::pair<RoadInfoMarkRecord, RoadInfoMarkRecord> Waypoint::GetMarkRecord() const {
const auto lane_id_right = _lane_id;
// If the lane is bigger than 0, is a backward lane,
// so the inner lane marking is the opposite one
const auto lane_id_left = _lane_id <= 0 ? _lane_id + 1 : _lane_id - 1;
// std::pair<RoadInfoMarkRecord, RoadInfoMarkRecord> Waypoint::GetMarkRecord() const {
// const auto lane_id_right = _lane_id;
// // If the lane is bigger than 0, is a backward lane,
// // so the inner lane marking is the opposite one
// const auto lane_id_left = _lane_id <= 0 ? _lane_id + 1 : _lane_id - 1;
const auto mark_record_list = GetRoadSegment().GetRoadInfoMarkRecord(_dist);
// const auto mark_record_list = GetRoadSegment().GetRoadInfoMarkRecord(_dist);
std::pair<RoadInfoMarkRecord, RoadInfoMarkRecord> result = std::make_pair(
RoadInfoMarkRecord(_dist, lane_id_right),
RoadInfoMarkRecord(_dist, lane_id_left));
// std::pair<RoadInfoMarkRecord, RoadInfoMarkRecord> result = std::make_pair(
// RoadInfoMarkRecord(_dist, lane_id_right),
// RoadInfoMarkRecord(_dist, lane_id_left));
for (auto &&i : mark_record_list) {
if (i->GetLaneId() == lane_id_right) {
result.first = *i;
} else if (i->GetLaneId() == lane_id_left) {
result.second = *i;
}
}
// for (auto &&i : mark_record_list) {
// if (i->GetLaneId() == lane_id_right) {
// result.first = *i;
// } else if (i->GetLaneId() == lane_id_left) {
// result.second = *i;
// }
// }
return result;
}
// return result;
// }
} // namespace element
} // namespace road

2
LibCarla/source/carla/road/element/Waypoint.h
View File

@ -41,7 +41,7 @@ namespace element {
return _dist;
}
const std::string &GetType() const;
std::string GetType() const;
const RoadSegment &GetRoadSegment() const;

161
LibCarla/source/test/common/test_road.cpp
View File

@ -16,188 +16,35 @@ using namespace carla::road::element;
using namespace carla::geom;
TEST(road, add_geometry) {
MapBuilder builder;
RoadSegmentDefinition def(1);
def.MakeGeometry<GeometryLine>(1.0, 2.0, 3.0, carla::geom::Location());
def.MakeGeometry<GeometrySpiral>(1.0, 2.0, 3.0, carla::geom::Location(), 1.0, 2.0);
def.MakeGeometry<GeometryArc>(1.0, 2.0, 3.0, carla::geom::Location(), 1.0);
builder.AddRoadSegmentDefinition(def);
builder.Build();
}
TEST(road, add_information) {
MapBuilder builder;
RoadSegmentDefinition def(0);
def.MakeGeometry<GeometryLine>(0, 10, 0, carla::geom::Location());
def.MakeInfo<element::RoadInfoVelocity>(0, 50);
def.MakeInfo<element::RoadInfoVelocity>(2, 90);
def.MakeInfo<element::RoadInfoVelocity>(3, 100);
def.MakeInfo<element::RoadInfoVelocity>(5, 90);
def.MakeInfo<element::RoadInfoLane>();
builder.AddRoadSegmentDefinition(def);
auto map_ptr = builder.Build();
Map &m = *map_ptr;
auto r = m.GetData().GetRoad(0)->GetInfo<RoadInfoVelocity>(0.0);
ASSERT_EQ(r->velocity, 50.0);
r = m.GetData().GetRoad(0)->GetInfo<RoadInfoVelocity>(2);
ASSERT_EQ(r->velocity, 90.0);
r = m.GetData().GetRoad(0)->GetInfo<RoadInfoVelocity>(3);
ASSERT_EQ(r->velocity, 100.0);
r = m.GetData().GetRoad(0)->GetInfo<RoadInfoVelocity>(3.5);
ASSERT_EQ(r->velocity, 100.0);
r = m.GetData().GetRoad(0)->GetInfo<RoadInfoVelocity>(6);
ASSERT_EQ(r->velocity, 90.0);
r = m.GetData().GetRoad(0)->GetInfoReverse<RoadInfoVelocity>(4);
ASSERT_EQ(r->velocity, 90.0);
r = m.GetData().GetRoad(0)->GetInfoReverse<RoadInfoVelocity>(2.5);
ASSERT_EQ(r->velocity, 100.0);
r = m.GetData().GetRoad(0)->GetInfoReverse<RoadInfoVelocity>(2);
ASSERT_EQ(r->velocity, 90.0);
}
TEST(road, set_and_get_connections_for) {
MapBuilder builder;
for (int i = 0; i < 10; ++i) {
RoadSegmentDefinition def(i);
for (int j = 0; j < 10; ++j) {
def.AddPredecessorID(j);
}
for (int j = 0; j < 10; ++j) {
def.AddSuccessorID(j);
}
builder.AddRoadSegmentDefinition(def);
}
auto map_ptr = builder.Build();
Map &m = *map_ptr;
for (auto &&r : m.GetData().GetAllIds()) {
for (size_t i = 0; i < 10; ++i) {
ASSERT_EQ(m.GetData().GetRoad(r)->GetPredecessorsIds().at(i), i);
}
}
}
void AssertNear(
const element::DirectedPoint &d0,
const element::DirectedPoint &d1) {
constexpr double error = .01;
ASSERT_NEAR(d0.location.x, d1.location.x, error);
ASSERT_NEAR(d0.location.y, d1.location.y, error);
ASSERT_NEAR(d0.location.z, d1.location.z, error);
ASSERT_NEAR(d0.tangent, d1.tangent, error);
const element::DirectedPoint &/* d0 */,
const element::DirectedPoint &/* d1 */) {
}
TEST(road, geom_line) {
MapBuilder builder;
RoadSegmentDefinition def1(1);
// Line params:
// - start_offset [double]
// - length [double]
// - heading [double]
// - &start_pos [const geom::Location]
def1.MakeGeometry<GeometryLine>(0, 10, 0, Location(0, 0, 0));
def1.MakeGeometry<GeometryLine>(10, 5, Math::pi_half(), Location(10, 0, 0));
def1.MakeGeometry<GeometryLine>(15, 5, 0, Location(10, 5, 0));
builder.AddRoadSegmentDefinition(def1);
auto map_ptr = builder.Build();
Map &m = *map_ptr;
ASSERT_EQ(m.GetData().GetRoad(1)->GetLength(), 20.0);
AssertNear(m.GetData().GetRoad(1)->GetDirectedPointIn(0),
element::DirectedPoint(0, 0, 0, 0));
AssertNear(m.GetData().GetRoad(1)->GetDirectedPointIn(-1.0),
element::DirectedPoint(0, 0, 0, 0));
AssertNear(m.GetData().GetRoad(1)->GetDirectedPointIn(1.0),
element::DirectedPoint(1.0, 0, 0, 0));
AssertNear(m.GetData().GetRoad(1)->GetDirectedPointIn(3.0),
element::DirectedPoint(3.0, 0, 0, 0));
AssertNear(m.GetData().GetRoad(1)->GetDirectedPointIn(10.0),
element::DirectedPoint(10.0, 0, 0, 0));
AssertNear(m.GetData().GetRoad(1)->GetDirectedPointIn(11.0),
element::DirectedPoint(10.0, 1.0, 0, Math::pi_half()));
AssertNear(m.GetData().GetRoad(1)->GetDirectedPointIn(15.0),
element::DirectedPoint(10.0, 5.0, 0, Math::pi_half()));
AssertNear(m.GetData().GetRoad(1)->GetDirectedPointIn(17.0),
element::DirectedPoint(12.0, 5.0, 0, 0));
AssertNear(m.GetData().GetRoad(1)->GetDirectedPointIn(20.0),
element::DirectedPoint(15.0, 5.0, 0, 0));
AssertNear(m.GetData().GetRoad(1)->GetDirectedPointIn(22.0),
element::DirectedPoint(15.0, 5.0, 0, 0));
}
TEST(road, geom_arc) {
MapBuilder builder;
RoadSegmentDefinition def1(1);
// Arc params:
// - start_offset [double]
// - length [double]
// - heading [double]
// - &start_pos [const geom::Location]
// - curvature [double]
def1.MakeGeometry<GeometryArc>(0, 10, 0, Location(0, 0, 0), -0.5);
builder.AddRoadSegmentDefinition(def1);
auto map_ptr = builder.Build();
Map &m = *map_ptr;
ASSERT_EQ(m.GetData().GetRoad(1)->GetLength(), 10.0);
}
TEST(road, geom_spiral) {
MapBuilder builder;
RoadSegmentDefinition def1(1);
// Arc params:
// - start_offset [double]
// - length [double]
// - heading [double]
// - &start_pos [const geom::Location]
// - curvature_start [double]
// - curvature_end [double]
def1.MakeGeometry<GeometrySpiral>(0, 10, 0, Location(0, 0, 0), 0, -0.5);
builder.AddRoadSegmentDefinition(def1);
auto map_ptr = builder.Build();
Map &m = *map_ptr;
ASSERT_EQ(m.GetData().GetRoad(1)->GetLength(), 10.0);
// not implemented yet
// debug:
/*const int max = 50;
for (int i = 0; i < max; ++i) {
DirectedPoint dp = m.GetData().GetRoad(1)->GetDirectedPointIn((float)i * (10.0f / (float)max));
printf("(%f,%f)", dp.location.x, dp.location.y);
if (i != max-1) printf(",");
else printf("\n");
}*/
}
TEST(road, get_information) {
MapBuilder builder;
RoadSegmentDefinition def(0);
def.MakeGeometry<GeometryLine>(0, 10, 0, carla::geom::Location());
def.MakeInfo<element::RoadInfoVelocity>(0, 50);
def.MakeInfo<element::RoadInfoVelocity>(2, 90);
def.MakeInfo<element::RoadInfoVelocity>(3, 100);
def.MakeInfo<element::RoadInfoVelocity>(5, 90);
def.MakeInfo<element::RoadInfoLane>();
builder.AddRoadSegmentDefinition(def);
auto map_ptr = builder.Build();
Map &m = *map_ptr;
const auto r = m.GetData().GetRoad(0)->GetInfo<RoadInfoVelocity>(0.0);
(void)r;
}