diff --git a/LibCarla/source/carla/opendrive/OpenDrive.cpp b/LibCarla/source/carla/opendrive/OpenDrive.cpp index e81cffda7..b2f4a55ce 100644 --- a/LibCarla/source/carla/opendrive/OpenDrive.cpp +++ b/LibCarla/source/carla/opendrive/OpenDrive.cpp @@ -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 &out_roads) { + static void fnc_generate_roads_data( + opendrive::types::OpenDriveData &openDriveRoad, + std::map &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>> &out_data) { + static void fnc_generate_junctions_data( + opendrive::types::OpenDriveData &openDriveRoad, + std::map>> &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 & 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 &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 & 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 &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 \ No newline at end of file +} // namespace carla diff --git a/LibCarla/source/carla/road/Types.h b/LibCarla/source/carla/road/Types.h index 1086bcbd5..46e0b4358 100644 --- a/LibCarla/source/carla/road/Types.h +++ b/LibCarla/source/carla/road/Types.h @@ -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() { diff --git a/LibCarla/source/test/test_road.cpp b/LibCarla/source/test/test_road.cpp index 0864b8548..2eb044f14 100644 --- a/LibCarla/source/test/test_road.cpp +++ b/LibCarla/source/test/test_road.cpp @@ -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); }