From 9e4a638117c39358cdb73e3936900c4fa48f02ad Mon Sep 17 00:00:00 2001 From: Manish Date: Mon, 18 Mar 2019 16:06:39 +0100 Subject: [PATCH] filling temporal map with lane information --- .../carla/opendrive/parser/LaneParser.cpp | 63 +++++++------ .../carla/opendrive/parser/LaneParser.h | 2 +- LibCarla/source/carla/road/MapBuilder.cpp | 88 ++++++++----------- LibCarla/source/carla/road/MapBuilder.h | 55 +++++------- 4 files changed, 91 insertions(+), 117 deletions(-) diff --git a/LibCarla/source/carla/opendrive/parser/LaneParser.cpp b/LibCarla/source/carla/opendrive/parser/LaneParser.cpp index 1a863385c..712ac47d0 100644 --- a/LibCarla/source/carla/opendrive/parser/LaneParser.cpp +++ b/LibCarla/source/carla/opendrive/parser/LaneParser.cpp @@ -16,42 +16,44 @@ namespace parser { void LaneParser::ParseLanes( road::RoadId road_id, - int lane_section_id, + float s, const pugi::xml_node &parent_node, carla::road::MapBuilder &map_builder) { for (pugi::xml_node lane_node : parent_node.children("lane")) { road::LaneId lane_id = lane_node.attribute("id").as_int(); + road::Lane *lane = map_builder.GetLane(road_id, lane_id, s); + // Lane Width for (pugi::xml_node lane_width_node : lane_node.children("width")) { - float s = lane_width_node.attribute("sOffset").as_float(); + float s_offset = lane_width_node.attribute("sOffset").as_float(); float a = lane_width_node.attribute("a").as_float(); float b = lane_width_node.attribute("b").as_float(); float c = lane_width_node.attribute("c").as_float(); float d = lane_width_node.attribute("d").as_float(); // Call Map builder create Lane Width function - map_builder.CreateLaneWidth(road_id, lane_section_id, lane_id, s, a, b, c, d); + map_builder.CreateLaneWidth(lane, s_offset, a, b, c, d); } // Lane Border for (pugi::xml_node lane_border_node : lane_node.children("border")) { - float s = lane_border_node.attribute("sOffset").as_float(); + float s_offset = lane_border_node.attribute("sOffset").as_float(); float a = lane_border_node.attribute("a").as_float(); float b = lane_border_node.attribute("b").as_float(); float c = lane_border_node.attribute("c").as_float(); float d = lane_border_node.attribute("d").as_float(); // Call Map builder create Lane Border function - map_builder.CreateLaneBorder(road_id, lane_section_id, lane_id, s, a, b, c, d); + map_builder.CreateLaneBorder(lane, s_offset, a, b, c, d); } // Lane Road Mark int road_mark_id = 0; for (pugi::xml_node lane_road_mark : lane_node.children("roadMark")) { - float s = lane_road_mark.attribute("sOffset").as_float(); + float s_offset = lane_road_mark.attribute("sOffset").as_float(); std::string type = lane_road_mark.attribute("type").value(); std::string weight = lane_road_mark.attribute("weight").value(); std::string color = lane_road_mark.attribute("color").value(); @@ -72,11 +74,9 @@ namespace parser { // Call map builder for LaneRoadMark map_builder.CreateRoadMark( - road_id, - lane_section_id, - lane_id, + lane, road_mark_id, - s, + s_offset, type, weight, color, @@ -92,20 +92,18 @@ namespace parser { float length = road_mark_type_line_node.attribute("length").as_float(); float space = road_mark_type_line_node.attribute("space").as_float(); float t = road_mark_type_line_node.attribute("tOffset").as_float(); - float s = road_mark_type_line_node.attribute("sOffset").as_float(); + float s_offset = road_mark_type_line_node.attribute("sOffset").as_float(); std::string rule = road_mark_type_line_node.attribute("rule").value(); float width = road_mark_type_line_node.attribute("width").as_float(); // Call map builder for LaneRoadMarkType LaneRoadMarkTypeLine map_builder.CreateRoadMarkTypeLine( - road_id, - lane_section_id, - lane_id, + lane, road_mark_id, length, space, t, - s, + s_offset, rule, width); } @@ -115,63 +113,63 @@ namespace parser { // Lane Material for (pugi::xml_node lane_material_node : lane_node.children("material")) { - float s = lane_material_node.attribute("sOffset").as_float(); + float s_offset = lane_material_node.attribute("sOffset").as_float(); std::string surface = lane_material_node.attribute("surface").value(); float friction = lane_material_node.attribute("friction").as_float(); float roughness = lane_material_node.attribute("roughness").as_float(); // Create map builder for Lane Material - map_builder.CreateLaneMaterial(road_id, lane_section_id, lane_id, s, surface, friction, roughness); + map_builder.CreateLaneMaterial(lane, s_offset, surface, friction, roughness); } // Lane Visibility for (pugi::xml_node lane_visibility_node : lane_node.children("visibility")) { - float s = lane_visibility_node.attribute("sOffset").as_float(); + float s_offset = lane_visibility_node.attribute("sOffset").as_float(); float forward = lane_visibility_node.attribute("forward").as_float(); float back = lane_visibility_node.attribute("back").as_float(); float left = lane_visibility_node.attribute("left").as_float(); float right = lane_visibility_node.attribute("right").as_float(); // Create map builder for Lane Visibility - map_builder.CreateLaneVisibility(road_id, lane_section_id, lane_id, s, forward, back, left, right); + map_builder.CreateLaneVisibility(lane, s_offset, forward, back, left, right); } // Lane Speed for (pugi::xml_node lane_speed_node : lane_node.children("speed")) { - float s = lane_speed_node.attribute("sOffset").as_float(); + float s_offset = lane_speed_node.attribute("sOffset").as_float(); float max = lane_speed_node.attribute("max").as_float(); std::string unit = lane_speed_node.attribute("unit").value(); // Create map builder for Lane Speed - map_builder.CreateLaneSpeed(road_id, lane_section_id, lane_id, s, max, unit); + map_builder.CreateLaneSpeed(lane, s_offset, max, unit); } // Lane Access for (pugi::xml_node lane_access_node : lane_node.children("access")) { - float s = lane_access_node.attribute("sOffset").as_float(); + float s_offset = lane_access_node.attribute("sOffset").as_float(); std::string restriction = lane_access_node.attribute("restriction").value(); // Create map builder for Lane Access - map_builder.CreateLaneAccess(road_id, lane_section_id, lane_id, s, restriction); + map_builder.CreateLaneAccess(lane, s_offset, restriction); } // Lane Height for (pugi::xml_node lane_height_node : lane_node.children("height")) { - float s = lane_height_node.attribute("sOffset").as_float(); + float s_offset = lane_height_node.attribute("sOffset").as_float(); float inner = lane_height_node.attribute("inner").as_float(); float outer = lane_height_node.attribute("outer").as_float(); // Create map builder for Lane Height - map_builder.CreateLaneHeight(road_id, lane_section_id, lane_id, s, inner, outer); + map_builder.CreateLaneHeight(lane, s_offset, inner, outer); } // Lane Rule for (pugi::xml_node lane_rule_node : lane_node.children("rule")) { - float s = lane_rule_node.attribute("sOffset").as_float(); + float s_offset = lane_rule_node.attribute("sOffset").as_float(); std::string value = lane_rule_node.attribute("value").value(); // Create map builder for Lane Height - map_builder.CreateLaneRule(road_id, lane_section_id, lane_id, s, value); + map_builder.CreateLaneRule(lane, s_offset, value); } } @@ -188,24 +186,23 @@ namespace parser { road::RoadId road_id = road_node.attribute("id").as_int(); for (pugi::xml_node lanes_node : road_node.children("lanes")) { - int lane_section_id = 0; - for (pugi::xml_node lane_section_node : lanes_node.children("laneSection")) { + for (pugi::xml_node lane_section_node : lanes_node.children("laneSection")) { + float s = lane_section_node.attribute("s").as_float(); pugi::xml_node left_node = lane_section_node.child("left"); if (left_node) { - ParseLanes(road_id, lane_section_id, left_node, map_builder); + ParseLanes(road_id, s, left_node, map_builder); } pugi::xml_node center_node = lane_section_node.child("center"); if (center_node) { - ParseLanes(road_id, lane_section_id, center_node, map_builder); + ParseLanes(road_id, s, center_node, map_builder); } pugi::xml_node right_node = lane_section_node.child("right"); if (right_node) { - ParseLanes(road_id, lane_section_id, right_node, map_builder); + ParseLanes(road_id, s, right_node, map_builder); } - ++lane_section_id; } } } diff --git a/LibCarla/source/carla/opendrive/parser/LaneParser.h b/LibCarla/source/carla/opendrive/parser/LaneParser.h index ec716bcdf..c9f38d49b 100644 --- a/LibCarla/source/carla/opendrive/parser/LaneParser.h +++ b/LibCarla/source/carla/opendrive/parser/LaneParser.h @@ -27,7 +27,7 @@ namespace parser { static void ParseLanes( road::RoadId road_id, - int lane_section_id, + float s, const pugi::xml_node &parent_node, carla::road::MapBuilder &map_builder); diff --git a/LibCarla/source/carla/road/MapBuilder.cpp b/LibCarla/source/carla/road/MapBuilder.cpp index a3e5452bd..d9e20a665 100644 --- a/LibCarla/source/carla/road/MapBuilder.cpp +++ b/LibCarla/source/carla/road/MapBuilder.cpp @@ -53,98 +53,81 @@ namespace road { // called from lane parser void MapBuilder::CreateLaneAccess( - const int32_t /*road_id*/, - const int32_t /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const float s, const std::string restriction) { - auto access = std::unique_ptr(new RoadInfoLaneAccess(s, restriction)); + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, restriction)); } void MapBuilder::CreateLaneBorder( - const int32_t /*road_id*/, - const int32_t /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const float s, const float a, const float b, const float c, const float d) { - auto border = std::unique_ptr(new RoadInfoLaneBorder(s, a, b, c, d)); + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, a, b, c, d)); } void MapBuilder::CreateLaneHeight( - const int32_t /*road_id*/, - const int32_t /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const float s, const float inner, const float outer) { - auto height = std::unique_ptr(new RoadInfoLaneHeight(s, inner, outer)); + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, inner, outer)); } void MapBuilder::CreateLaneMaterial( - const int32_t /*road_id*/, - const int32_t /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const float s, const std::string surface, const float friction, const float roughness) { - auto material = - std::unique_ptr(new RoadInfoLaneMaterial(s, surface, friction, roughness)); + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, surface, friction, + roughness)); } void MapBuilder::CreateLaneOffset( - const int32_t /*road_id*/, - const int32_t /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const float s, const float a, const float b, const float c, const float d) { - auto offset = std::unique_ptr(new RoadInfoLaneOffset(s, a, b, c, d)); + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, a, b, c, d)); } void MapBuilder::CreateLaneRule( - const int32_t /*road_id*/, - const int32_t /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const float s, const std::string value) { - auto rule = std::unique_ptr(new RoadInfoLaneRule(s, value)); + + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, value)); } void MapBuilder::CreateLaneVisibility( - const int32_t /*road_id*/, - const int32_t /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const float s, const float forward, const float back, const float left, const float right) { - auto visibility = - std::unique_ptr(new RoadInfoLaneVisibility(s, forward, back, left, right)); + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, forward, back, + left, right)); } void MapBuilder::CreateLaneWidth( - const int32_t /*road_id*/, - const int32_t /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const float s, const float a, const float b, const float c, const float d) { - auto width = std::unique_ptr(new RoadInfoLaneWidth(s, a, b, c, d)); + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, a, b, c, d)); } void MapBuilder::CreateRoadMark( - const int32_t /*road_id*/, - const int32_t /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const int road_mark_id, const float s, const std::string type, @@ -169,15 +152,13 @@ namespace road { } else { lc = RoadInfoMarkRecord::LaneChange::None; } - auto mark = - std::unique_ptr(new RoadInfoMarkRecord(s, road_mark_id, type, weight, color, + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, road_mark_id, type, + weight, color, material, width, lc, height, type_name, type_width)); } void MapBuilder::CreateRoadMarkTypeLine( - const int32_t /*road_id*/, - const int32_t /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const int road_mark_id, const float length, const float space, @@ -185,8 +166,8 @@ namespace road { const float s, const std::string rule, const float width) { - auto line = - std::unique_ptr(new RoadInfoMarkTypeLine(s, road_mark_id, length, space, + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, road_mark_id, + length, space, tOffset, rule, width)); // Find the parent road mark record using the ids provided and then add this // line to its lise of lines @@ -195,13 +176,11 @@ namespace road { } void MapBuilder::CreateLaneSpeed( - const int32_t /*road_id*/, - const int /*lane_section_id*/, - const int32_t /*lane_id*/, + const Lane *lane, const float s, const float max, const std::string /*unit*/) { - auto speed = std::unique_ptr(new RoadInfoVelocity(s, max)); + _temp_lane_info_container[lane].emplace_back(std::make_unique(s, max)); } void MapBuilder::AddSignal( @@ -377,9 +356,18 @@ namespace road { const uint32_t signal_id, const uint32_t dependency_id, const std::string dependency_type) { - _map_data.GetRoad(road_id)->GetSignal(signal_id)->AddDependency(signal::SignalDependency(road_id, - signal_id, dependency_id, dependency_type)); + _map_data.GetRoad(road_id)->GetSignal(signal_id)->AddDependency(signal::SignalDependency( + road_id, + signal_id, + dependency_id, + dependency_type)); + } + Lane *MapBuilder::GetLane( + const RoadId road_id, + const LaneId lane_id, + const float s) { + return _map_data.GetLane(road_id, lane_id, s); } } // namespace road diff --git a/LibCarla/source/carla/road/MapBuilder.h b/LibCarla/source/carla/road/MapBuilder.h index 76386cd51..f97da12dc 100644 --- a/LibCarla/source/carla/road/MapBuilder.h +++ b/LibCarla/source/carla/road/MapBuilder.h @@ -204,16 +204,12 @@ namespace road { // called from lane parser void CreateLaneAccess( - const int32_t road_id, - const int32_t lane_section_id, - const int32_t lane_id, + const Lane* lane, const float s, const std::string restriction); void CreateLaneBorder( - const int32_t road_id, - const int32_t lane_section_id, - const int32_t lane_id, + const Lane* lane, const float s, const float a, const float b, @@ -221,26 +217,20 @@ namespace road { const float d); void CreateLaneHeight( - const int32_t road_id, - const int32_t lane_section_id, - const int32_t lane_id, + const Lane* lane, const float s, const float inner, const float outer); void CreateLaneMaterial( - const int32_t road_id, - const int32_t lane_section_id, - const int32_t lane_id, + const Lane* lane, const float s, const std::string surface, const float friction, const float roughness); void CreateLaneOffset( - const int32_t road_id, - const int32_t lane_section_id, - const int32_t lane_id, + const Lane* lane, const float s, const float a, const float b, @@ -248,16 +238,12 @@ namespace road { const float d); void CreateLaneRule( - const int32_t road_id, - const int32_t lane_section_id, - const int32_t lane_id, + const Lane* lane, const float s, const std::string value); void CreateLaneVisibility( - const int32_t road_id, - const int32_t lane_section_id, - const int32_t lane_id, + const Lane* lane, const float s, const float forward, const float back, @@ -265,9 +251,7 @@ namespace road { const float right); void CreateLaneWidth( - const int32_t road_id, - const int32_t lane_section_id, - const int32_t lane_id, + const Lane* lane, const float s, const float a, const float b, @@ -275,9 +259,7 @@ namespace road { const float d); void CreateRoadMark( - const int32_t road_id, - const int32_t lane_section_id, - const int32_t lane_id, + const Lane* lane, const int road_mark_id, const float s, const std::string type, @@ -291,9 +273,7 @@ namespace road { const float type_width); void CreateRoadMarkTypeLine( - const int32_t road_id, - const int32_t lane_section_id, - const int32_t lane_id, + const Lane* lane, const int road_mark_id, const float length, const float space, @@ -303,9 +283,7 @@ namespace road { const float width); void CreateLaneSpeed( - const int32_t road_id, - const int lane_section_id, - const int32_t lane_id, + const Lane* lane, const float s, const float max, const std::string unit); @@ -335,6 +313,11 @@ namespace road { const uint32_t dependency_id, const std::string dependency_type); + Lane *GetLane( + const RoadId road_id, + const LaneId lane_id, + const float s); + private: MapData _map_data; @@ -344,6 +327,12 @@ namespace road { /// Create the pointers between RoadSegments based on the ids void CreatePointersBetweenRoadSegments(); + + private: + /// Map to temporary store all the lane infos until the map is built, so they + /// can be added all together + std::unordered_map>> + _temp_lane_info_container; }; } // namespace road