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

Add geometry to road segment

This commit is contained in:
iFuSiiOnzZ 2018-10-10 12:59:48 +02:00
parent 627ccaf6c2
commit 7c8dc9907c
3 changed files with 92 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -6,20 +6,26 @@
namespace carla {
namespace opendrive {
#define UNUSED(x) (void)x
#define UNUSED(x) (void) x
struct lane_junction_t {
int connection_road = -1;
int from_lane = 0, to_lane = 0;
};
static void fnc_generate_roads_data(opendrive::types::OpenDriveData &openDriveRoad, std::map<int, opendrive::types::RoadInformation *> &out_roads) {
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<lane_junction_t>>> &out_data) {
static void fnc_generate_junctions_data(
opendrive::types::OpenDriveData &openDriveRoad,
std::map<int,
std::map<int, std::vector<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) {
lane_junction_t junctionData;
@ -61,34 +67,80 @@ namespace opendrive {
fnc_generate_junctions_data(open_drive_road, junctionsData);
// Transforma data for the MapBuilder
for(road_data_t::iterator it = roadData.begin(); it != roadData.end(); ++it)
{
for (road_data_t::iterator it = roadData.begin(); it != roadData.end(); ++it) {
carla::road::RoadSegmentDefinition roadSegment(it->first);
if(it->second->road_link.successor != nullptr)
{
if(it->second->road_link.successor->element_type == "junction")
{
std::vector<lane_junction_t> & options = junctionsData[it->second->road_link.successor->id][it->first];
for(size_t i = 0; i < options.size(); ++i) roadSegment.AddSuccessorID(options[i].connection_road);
}
else
{
if (it->second->road_link.successor != nullptr) {
if (it->second->road_link.successor->element_type == "junction") {
std::vector<lane_junction_t> &options =
junctionsData[it->second->road_link.successor->id][it->first];
for (size_t i = 0; i < options.size(); ++i) {
roadSegment.AddSuccessorID(options[i].connection_road);
}
} else {
roadSegment.AddSuccessorID(it->second->road_link.successor->id);
}
}
if(it->second->road_link.predecessor != nullptr)
{
if(it->second->road_link.predecessor->element_type == "junction")
{
std::vector<lane_junction_t> & options = junctionsData[it->second->road_link.predecessor->id][it->first];
for(size_t i = 0; i < options.size(); ++i) roadSegment.AddPredecessorID(options[i].connection_road);
if (it->second->road_link.predecessor != nullptr) {
std::vector<lane_junction_t> &options =
junctionsData[it->second->road_link.predecessor->id][it->first];
for (size_t i = 0; i < options.size(); ++i) {
roadSegment.AddPredecessorID(options[i].connection_road);
}
}
else
{
roadSegment.AddPredecessorID(it->second->road_link.predecessor->id);
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];
carla::road::GeometryArc *newarc = new carla::road::GeometryArc(arc->curvature,
arc->start_position,
arc->length,
arc->heading,
loc);
roadSegment.AddGeometry(*newarc);
break;
}
case carla::opendrive::types::GeometryType::LINE: {
carla::opendrive::types::GeometryAttributesLine *line =
(carla::opendrive::types::GeometryAttributesLine *) it->second->geometry_attributes[i];
carla::road::GeometryLine *newline = new carla::road::GeometryLine(line->start_position,
line->length,
line->heading,
loc);
roadSegment.AddGeometry(*newline);
break;
}
case carla::opendrive::types::GeometryType::SPIRAL: {
carla::opendrive::types::GeometryAttributesSpiral *spiral =
(carla::opendrive::types::GeometryAttributesSpiral *) it->second->geometry_attributes[i];
carla::road::GeometrySpiral *newspiral = new carla::road::GeometrySpiral(spiral->curve_start,
spiral->curve_end,
spiral->start_position,
spiral->length,
spiral->heading,
loc);
roadSegment.AddGeometry(*newspiral);
break;
}
default: {
break;
}
}
}
mapBuilder.AddRoadSegment(roadSegment);
}
}
@ -99,5 +151,6 @@ namespace opendrive {
UNUSED(input);
return road::Map();
}
} // namespace opendrive
} // namespace carla
} // namespace carla

21
LibCarla/source/carla/road/Types.h
View File

@ -47,24 +47,31 @@ namespace road {
double _start_position_offset; // s-offset [meters]
double _heading; // start orientation [radians]
geom::Location start_position; // [meters]
geom::Location _start_position; // [meters]
protected:
Geometry(GeometryType type) : _type(type) {}
Geometry(GeometryType type, double start_offset, double length, double heading, const geom::Location &start_pos) :
_type(type),
_length(length),
_start_position_offset(start_offset),
_heading(heading),
_start_position(start_pos)
{}
};
class GeometryLine : public Geometry {
public:
GeometryLine() : Geometry(GeometryType::LINE) {}
GeometryLine(double start_offset, double length, double heading, const geom::Location &start_pos) :
Geometry(GeometryType::LINE, start_offset, length, heading, start_pos) {}
};
class GeometryArc : public Geometry {
public:
GeometryArc(double curv)
: Geometry(GeometryType::ARC),
GeometryArc(double curv, double start_offset, double length, double heading, const geom::Location &start_pos)
: Geometry(GeometryType::ARC, start_offset, length, heading, start_pos),
_curvature(curv) {}
double GetCurvature() {
return _curvature;
@ -78,8 +85,8 @@ namespace road {
class GeometrySpiral : public Geometry {
public:
GeometrySpiral(double curv_s, double curv_e)
: Geometry(GeometryType::SPIRAL),
GeometrySpiral(double curv_s, double curv_e, double start_offset, double length, double heading, const geom::Location &start_pos)
: Geometry(GeometryType::SPIRAL, start_offset, length, heading, start_pos),
_curve_start(curv_s),
_curve_end(curv_e) {}
double GetCurveStart() {

14
LibCarla/source/test/test_road.cpp
View File

@ -14,18 +14,4 @@
using namespace carla::road;
TEST(road, compilation_test) {
Map m;
RoadSegmentDefinition def(1);
Geometry geom[3] = {
GeometryLine(),
GeometrySpiral(1.0, 2.0),
GeometryArc(1.0)
};
def.AddGeometry(geom[0]);
def.AddGeometry(geom[1]);
def.AddGeometry(geom[2]);
m.AddRoadSegment(def);
}