From e6fc38a98065ce6d4e77703d42d65f1fac36f1bd Mon Sep 17 00:00:00 2001 From: bernatx Date: Fri, 15 Nov 2019 10:40:11 +0100 Subject: [PATCH] Parse 'crosswalks' on openDRIVE --- LibCarla/source/carla/client/Map.cpp | 4 + LibCarla/source/carla/client/Map.h | 2 + .../carla/opendrive/OpenDriveParser.cpp | 2 +- .../carla/opendrive/parser/ObjectParser.cpp | 282 +++--------------- LibCarla/source/carla/road/Map.cpp | 66 +++- LibCarla/source/carla/road/Map.h | 2 + LibCarla/source/carla/road/MapBuilder.cpp | 26 ++ LibCarla/source/carla/road/MapBuilder.h | 18 ++ LibCarla/source/carla/road/ObjectSet.h | 53 ++++ LibCarla/source/carla/road/Road.h | 15 + .../source/carla/road/element/RoadObject.h | 43 +++ .../carla/road/element/RoadObjectCrosswalk.h | 79 +++++ .../carla/road/element/RoadObjectIterator.h | 105 +++++++ .../carla/road/element/RoadObjectVisitor.h | 33 ++ PythonAPI/carla/source/libcarla/Map.cpp | 1 + 15 files changed, 491 insertions(+), 240 deletions(-) create mode 100644 LibCarla/source/carla/road/ObjectSet.h create mode 100644 LibCarla/source/carla/road/element/RoadObject.h create mode 100644 LibCarla/source/carla/road/element/RoadObjectCrosswalk.h create mode 100644 LibCarla/source/carla/road/element/RoadObjectIterator.h create mode 100644 LibCarla/source/carla/road/element/RoadObjectVisitor.h diff --git a/LibCarla/source/carla/client/Map.cpp b/LibCarla/source/carla/client/Map.cpp index 40b2ff2ca..37010bc3f 100644 --- a/LibCarla/source/carla/client/Map.cpp +++ b/LibCarla/source/carla/client/Map.cpp @@ -97,5 +97,9 @@ namespace client { return _map.GetGeoReference(); } + std::vector Map::GetAllCrosswalkZones() const{ + return _map.GetAllCrosswalkZones(); + } + } // namespace client } // namespace carla diff --git a/LibCarla/source/carla/client/Map.h b/LibCarla/source/carla/client/Map.h index 0c1926b97..17150c021 100644 --- a/LibCarla/source/carla/client/Map.h +++ b/LibCarla/source/carla/client/Map.h @@ -65,6 +65,8 @@ namespace client { const geom::GeoLocation &GetGeoReference() const; + std::vector GetAllCrosswalkZones() const; + private: const rpc::MapInfo _description; diff --git a/LibCarla/source/carla/opendrive/OpenDriveParser.cpp b/LibCarla/source/carla/opendrive/OpenDriveParser.cpp index c2c43f1b4..ae004692d 100644 --- a/LibCarla/source/carla/opendrive/OpenDriveParser.cpp +++ b/LibCarla/source/carla/opendrive/OpenDriveParser.cpp @@ -42,7 +42,7 @@ namespace opendrive { parser::ProfilesParser::Parse(xml, map_builder); parser::TrafficGroupParser::Parse(xml, map_builder); parser::SignalParser::Parse(xml, map_builder); - // parser::ObjectParser::Parse(xml, map_builder); + parser::ObjectParser::Parse(xml, map_builder); return map_builder.Build(); } diff --git a/LibCarla/source/carla/opendrive/parser/ObjectParser.cpp b/LibCarla/source/carla/opendrive/parser/ObjectParser.cpp index 60d256dd8..a2b43e846 100644 --- a/LibCarla/source/carla/opendrive/parser/ObjectParser.cpp +++ b/LibCarla/source/carla/opendrive/parser/ObjectParser.cpp @@ -7,6 +7,8 @@ #include "carla/opendrive/parser/ObjectParser.h" #include "carla/road/MapBuilder.h" +#include "carla/road/element/RoadObjectCrosswalk.h" +#include "carla/road/Road.h" #include @@ -14,246 +16,56 @@ namespace carla { namespace opendrive { namespace parser { - using RoadID = std::string; - using ObjectID = std::string; - using DependencyID = std::string; - using SignalReferenceID = std::string; - - template - static void AddValidity( - pugi::xml_node parent_node, - const std::string &node_name, - const ObjectID &objectID, - T &&function) { - for (pugi::xml_node validity_node = parent_node.child(node_name.c_str()); - validity_node; - validity_node = validity_node.next_sibling("validity")) { - const int from_lane = validity_node.attribute("fromLane").as_int(); - const int to_lane = validity_node.attribute("toLane").as_int(); - log_debug("Added validity to signal ", objectID, ":", from_lane, to_lane); - function(objectID, from_lane, to_lane); - } - } - void ObjectParser::Parse( - const pugi::xml_document & /*xml*/, - carla::road::MapBuilder & /* map_builder */) { - throw_exception(std::runtime_error("ObjectParser not ready")); - // Extracting the OpenDRIVE - // pugi::xml_node opendrive_node = xml.child("OpenDRIVE"); - // for (pugi::xml_node road_node = opendrive_node.child("road"); - // road_node; - // road_node = road_node.next_sibling("road")) { - // const RoadID road_id = static_cast(road_node.attribute("id").value()); - // const pugi::xml_node objects_node = road_node.child("objects"); - // for (pugi::xml_node object_node = objects_node.child("object"); - // object_node; - // object_node = object_node.next_sibling("object")) { - // const std::string type = object_node.attribute("type").value(); - // const std::string name = object_node.attribute("name").value(); - // const ObjectID object_id = object_node.attribute("id").value(); - // const double s_position = object_node.attribute("s").as_double(); - // const double t_position = object_node.attribute("t").as_double(); - // const double z_offset = object_node.attribute("zOffset").as_double(); - // const double valid_length = object_node.attribute("validLength").as_double(); - // const std::string orientation = object_node.attribute("orientation").value(); - // const double lenght = object_node.attribute("lenght").as_double(); - // const double width = object_node.attribute("width").as_double(); - // const double radius = object_node.attribute("radius").as_double(); - // const double height = object_node.attribute("height").as_double(); - // const double hdg = object_node.attribute("hdg").as_double(); - // const double pitch = object_node.attribute("pitch").as_double(); - // const double roll = object_node.attribute("roll").as_double(); - // /*map_builder.AddObjectInRoad(road_id, type, name, object_id, - // s_position, t_position, z_offset, valid_length, - // orientation, lenght, width, radius, height, hdg, pitch, roll);*/ - // log_debug("AddObjectInRoad", - // road_id, - // type, - // name, - // object_id, - // s_position, - // t_position, - // z_offset, - // valid_length, - // orientation, - // lenght, - // width, - // radius, - // height, - // hdg, - // pitch, - // roll); - // for (pugi::xml_node repeat_node = object_node.child("repeat"); - // repeat_node; - // repeat_node = repeat_node.next_sibling("repeat")) { - // const double s_position_rep = repeat_node.attribute("s").as_double(); - // const double lenght_rep = repeat_node.attribute("lenght").as_double(); - // const double distance_rep = repeat_node.attribute("distance").as_double(); - // const double t_start_rep = repeat_node.attribute("tStart").as_double(); - // const double t_end_rep = repeat_node.attribute("tEnd").as_double(); - // const double width_start_rep = repeat_node.attribute("widthStart").as_double(); - // const double width_end_rep = repeat_node.attribute("widthEnd").as_double(); - // const double height_start_rep = repeat_node.attribute("heightStart").as_double(); - // const double z_offset_start_rep = repeat_node.attribute("zOffsetStart").as_double(); - // const double z_offset_end_rep = repeat_node.attribute("zOffsetEnd").as_double(); - // /*map_builder.AddRepeatRecordInObject(object_id, s_position_rep, - // lenght_rep, t_start_rep, - // t_start_rep, t_end_rep, width_start_rep, width_end_rep, - // height_start_rep, z_offset_start_rep, - // z_offset_end_rep); - // */ - // log_debug("AddRepeatRecordInObject", object_id, s_position_rep, lenght_rep, distance_rep, - // t_start_rep, t_end_rep, width_start_rep, width_end_rep, height_start_rep, z_offset_start_rep, - // z_offset_end_rep); + const pugi::xml_document &xml, + carla::road::MapBuilder &map_builder) { + + std::vector points; + + for (pugi::xml_node node_road : xml.child("OpenDRIVE").children("road")) { - // } - // pugi::xml_node outline_node = object_node.child("outline"); - // for (pugi::xml_node corner_road_node = outline_node.child("cornerRoad"); - // corner_road_node; - // corner_road_node = corner_road_node.next_sibling("cornerRoad")) { - // const double s_position_corner_road = corner_road_node.attribute("s").as_double(); - // const double t_position_corner_road = corner_road_node.attribute("t").as_double(); - // const double dz_position_corner_road = corner_road_node.attribute("dz").as_double(); - // const double height_position_corner_road = corner_road_node.attribute("height").as_double(); - // /* - // map_builder.AddCornerRoadToObject(object_id, - // s_position_corner_road, t_position_corner_road, - // dz_position_corner_road, height_position_corner_road); - // */ - // log_debug("AddCornerRoadToObject", object_id, s_position_corner_road, t_position_corner_road, - // dz_position_corner_road, height_position_corner_road); - // } - // for (pugi::xml_node corner_local_node = outline_node.child("cornerLocal"); - // corner_local_node; - // corner_local_node = corner_local_node.next_sibling("cornerLocal")) { - // const double u_coordinate_corner_local = corner_local_node.attribute("u").as_double(); - // const double v_coordinate_corner_local = corner_local_node.attribute("v").as_double(); - // const double z_coordinate_corner_local = corner_local_node.attribute("z").as_double(); - // const double height_corner_local = corner_local_node.attribute("height").as_double(); - // /* - // map_builder.AddCornerLocalToObject(object_id, - // u_coordinate_corner_local, v_coordinate_corner_local, - // z_coordinate_corner_local, height_corner_local); - // */ - // log_debug("AddCornerLocalToObject", object_id, u_coordinate_corner_local, v_coordinate_corner_local, - // z_coordinate_corner_local, height_corner_local); - // } - // pugi::xml_node material_node = object_node.child("material"); - // if (material_node != nullptr) { - // const std::string surface_material = material_node.attribute("surface").value(); - // const double friction_material = material_node.attribute("friction").as_double(); - // const double roughness_material = material_node.attribute("roughness").as_double(); - // /* - // map_builder.AddMaterialToObject(object_id, surface_material, - // friction_material, roughness_material); - // */ - // log_debug("AddMaterialToObject", object_id, surface_material, friction_material, - // roughness_material); - // } + // parse all objects + pugi::xml_node node_objects = node_road.child("objects"); + if (node_objects) { - // /* AddValidity(object_node, "validity", object_id, - // ([&map_builder](const ObjectID &object_id, const int16_t from_lane, - // const int16_t to_lane) - // { map_builder.AddValidityToObject(object_id, from_lane, - // to_lane);})); */ + for (pugi::xml_node node_object : node_objects.children("object")) { + + // type Crosswalk + std::string type = node_object.attribute("type").as_string(); + if (type == "crosswalk") { - // pugi::xml_node parking_space_node = object_node.child("parkingSpace"); - // if (parking_space_node != nullptr) { - // const std::string access_parking = parking_space_node.attribute("access").value(); - // const std::string restrictions_parking = parking_space_node.attribute("restrictions").value(); - // /* - // map_builder.AddParkingSpaceToObject(object_id, access_parking, - // restrictions_parking); - // */ - // log_debug("AddParkingSpaceToObject", object_id, access_parking, restrictions_parking); - // } - // for (pugi::xml_node marking_node = parking_space_node.child("marking"); - // marking_node; - // marking_node = marking_node.next_sibling("marking")) { - // const std::string side_marking = marking_node.attribute("side").value(); - // const std::string type_marking = marking_node.attribute("type").value(); - // const double width_marking = marking_node.attribute("width").as_double(); - // const std::string color_marking = marking_node.attribute("color").value(); - // /* - // Problem, there is no "Parking ID" or anything similar... how do we - // link them? - // map_builder.AddMarkingToParkingSpace(side_marking, type_marking, - // width_marking, color_marking); - // */ - // log_debug("AddMarkingToParkingSpace", side_marking, type_marking, - // width_marking, color_marking); - // } - // for (pugi::xml_node obj_reference_node = object_node.child("objectReference"); - // obj_reference_node; - // obj_reference_node = obj_reference_node.next_sibling("objectReference")) { - // const double s_position_obj_ref = obj_reference_node.attribute("s").as_double(); - // const double t_position_obj_ref = obj_reference_node.attribute("t").as_double(); - // const ObjectID id_obj_ref = obj_reference_node.attribute("id").value(); - // const double z_offset_obj_ref = obj_reference_node.attribute("zOffset").as_double(); - // const double valid_lenght_obj_ref = obj_reference_node.attribute("validLenght").as_double(); - // const std::string orientation_obj_ref = obj_reference_node.attribute("orientation").value(); - // /* - // map_builder.AddObjectReferenceToObject(object_id, - // s_position_obj_ref, t_position_obj_ref, - // id_obj_ref, z_offset_obj_ref, valid_lenght_obj_ref, - // orientation_obj_ref); - // */ - // log_debug("AddObjectReferenceToObject", object_id, s_position_obj_ref, t_position_obj_ref, - // id_obj_ref, z_offset_obj_ref, valid_lenght_obj_ref, orientation_obj_ref); - // /* AddValidity(obj_reference_node, "validity", id_obj_ref, - // ([&map_builder](const ObjectID &id_obj_ref, const int16_t - // from_lane, const int16_t to_lane) - // { map_builder.AddValidityToObjectRef(id_obj_ref, from_lane, - // to_lane);})); */ - // } + // read all points + pugi::xml_node node_outline = node_object.child("outline"); + if (node_outline) { + points.clear(); + for (pugi::xml_node node_corner : node_outline.children("cornerLocal")) { + points.emplace_back(node_corner.attribute("u").as_double(), + node_corner.attribute("v").as_double(), + node_corner.attribute("z").as_double()); + } + } + // get road id + road::RoadId road_id = node_road.attribute("id").as_uint(); + road::Road *road = map_builder.GetRoad(road_id); + + // create the object + map_builder.AddRoadObjectCrosswalk(road, + node_object.attribute("name").as_string(), + node_object.attribute("s").as_double(), + node_object.attribute("t").as_double(), + node_object.attribute("zOffset").as_double(), + node_object.attribute("hdg").as_double(), + node_object.attribute("pitch").as_double(), + node_object.attribute("roll").as_double(), + node_object.attribute("orientation").as_string(), + node_object.attribute("width").as_double(), + node_object.attribute("length").as_double(), + points); - // } - // for (pugi::xml_node tunnel_node = objects_node.child("tunnel"); - // tunnel_node; - // tunnel_node = tunnel_node.next_sibling("tunnel")) { - // const double s_position_tunnel = tunnel_node.attribute("s").as_double(); - // const double lenght_tunnel = tunnel_node.attribute("lenght").as_double(); - // const std::string name_tunnel = tunnel_node.attribute("name").value(); - // const ObjectID id_tunnel = tunnel_node.attribute("id").value(); - // const std::string type_tunnel = tunnel_node.attribute("type").value(); - // const double lightning_tunnel = tunnel_node.attribute("lightning").as_double(); - // const double daylight_tunnel = tunnel_node.attribute("daylight").as_double(); - // /* - // map_builder.AddTunnel(s_position_tunnel, lenght_tunnel, name_tunnel, - // id_tunnel, - // type_tunnel, lightning_tunnel, daylight_tunnel); - // */ - // log_debug("AddTunnel", s_position_tunnel, lenght_tunnel, name_tunnel, id_tunnel, - // type_tunnel, lightning_tunnel, daylight_tunnel); - // /* AddValidity(tunnel_node, "validity", id_tunnel, - // ([&map_builder](const ObjectID &id_tunnel, const int16_t from_lane, - // const int16_t to_lane) - // { map_builder.AddValidityToTunnel(id_tunnel, from_lane, - // to_lane);})); */ - // } - // for (pugi::xml_node bridge_node = objects_node.child("bridge"); - // bridge_node; - // bridge_node = bridge_node.next_sibling("bridge")) { - // const double s_position_bridge = bridge_node.attribute("s").as_double(); - // const double lenght_bridge = bridge_node.attribute("lenght").as_double(); - // const std::string name_bridge = bridge_node.attribute("name").value(); - // const ObjectID id_bridge = bridge_node.attribute("id").value(); - // const std::string type_bridge = bridge_node.attribute("type").value(); - // /* - // map_builder.AddBridge(s_position_bridge, lenght_bridge, name_bridge, - // id_bridge, - // type_bridge); - // */ - // log_debug("AddTunnel", s_position_bridge, lenght_bridge, name_bridge, id_bridge, - // type_bridge); - // /*AddValidity(bridge_node, "validity", id_bridge, - // ([&map_builder](const ObjectID &id_bridge, const int16_t from_lane, - // const int16_t to_lane) - // { map_builder.AddValidityToBridge(id_bridge, from_lane, - // to_lane);}));*/ - // } - // } + } + } + } + } } } // namespace parser } // namespace opendrive diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index ddc544d3f..f585f3587 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -12,6 +12,7 @@ #include "carla/road/element/RoadInfoLaneWidth.h" #include "carla/road/element/RoadInfoMarkRecord.h" #include "carla/road/element/RoadInfoLaneOffset.h" +#include "carla/road/element/RoadObjectCrosswalk.h" #include "carla/geom/Math.h" #include @@ -107,6 +108,10 @@ namespace road { const T container, const double s, const LaneId lane_id) { + + // lane_id can't be 0 + RELEASE_ASSERT(lane_id != 0); + const bool negative_lane_id = lane_id < 0; double dist = 0.0; double tangent = 0.0; @@ -240,9 +245,6 @@ namespace road { } geom::Transform Map::ComputeTransform(Waypoint waypoint) const { - // lane_id can't be 0 - RELEASE_ASSERT(waypoint.lane_id != 0); - const auto &road = _data.GetRoad(waypoint.road_id); // must s be smaller (or eq) than road lenght and bigger (or eq) than 0? @@ -259,6 +261,7 @@ namespace road { float lane_width = 0.0f; float lane_tangent = 0.0f; + if (waypoint.lane_id < 0) { // right lane const auto side_lanes = MakeListView( @@ -267,7 +270,7 @@ namespace road { ComputeTotalLaneWidth(side_lanes, waypoint.s, waypoint.lane_id); lane_width = static_cast(computed_width.first); lane_tangent = static_cast(computed_width.second); - } else { + } else if (waypoint.lane_id > 0) { // left lane const auto side_lanes = MakeListView(lanes.lower_bound(1), lanes.end()); const auto computed_width = @@ -363,6 +366,61 @@ namespace road { return LaneCrossingCalculator::Calculate(*this, origin, destination); } + std::vector Map::GetAllCrosswalkZones() const { + std::vector result; + + for (const auto &pair : _data.GetRoads()) { + const auto &road = pair.second; + std::vector crosswalks = road.GetObjects(); + if (crosswalks.size() > 0) { + for (auto crosswalk : crosswalks) { + // waypoint only at start position + std::vector points; + Waypoint waypoint; + geom::Transform base; + for (const auto §ion : road.GetLaneSectionsAt(crosswalk->GetS())) { + // get the section with the center lane + for (const auto &lane : section.GetLanes()) { + // is the center line + if (lane.first == 0) { + // get the center point + waypoint.road_id = pair.first; + waypoint.section_id = section.GetId(); + waypoint.lane_id = 0; + waypoint.s = crosswalk->GetS(); + base = ComputeTransform(waypoint); + } + } + } + + // move perpendicular ('t') + geom::Transform pivot = base; + pivot.rotation.yaw -= geom::Math::ToDegrees(static_cast(crosswalk->GetHeading())); + pivot.rotation.yaw -= 90; // move perpendicular to 's' for the lateral offset + geom::Vector3D v(static_cast(crosswalk->GetT()), 0.0f, 0.0f); + pivot.TransformPoint(v); + // restore pivot position and orientation + pivot = base; + pivot.location = v; + pivot.rotation.yaw -= geom::Math::ToDegrees(static_cast(crosswalk->GetHeading())); + + // calculate all the corners + for (auto corner : crosswalk->GetPoints()) { + geom::Vector3D v2(static_cast(corner.u), static_cast(corner.v), static_cast(corner.z)); + // set the width larger to contact with the sidewalk (in case they have gutter area) + if (corner.u < 0) + v2.x -= 1.0f; + else + v2.x += 1.0f; + pivot.TransformPoint(v2); + result.push_back(v2); + } + } + } + } + return result; + } + // =========================================================================== // -- Map: Waypoint generation ----------------------------------------------- // =========================================================================== diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index 360b8591a..e39e543b1 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -75,6 +75,8 @@ namespace road { const geom::Location &origin, const geom::Location &destination) const; + std::vector GetAllCrosswalkZones() const; + /// ======================================================================== /// -- Waypoint generation ------------------------------------------------- /// ======================================================================== diff --git a/LibCarla/source/carla/road/MapBuilder.cpp b/LibCarla/source/carla/road/MapBuilder.cpp index b41c3e8d3..86740b1a3 100644 --- a/LibCarla/source/carla/road/MapBuilder.cpp +++ b/LibCarla/source/carla/road/MapBuilder.cpp @@ -20,6 +20,8 @@ #include "carla/road/element/RoadInfoMarkTypeLine.h" #include "carla/road/element/RoadInfoSpeed.h" #include "carla/road/element/RoadInfoVisitor.h" +#include "carla/road/element/RoadObjectCrosswalk.h" +#include "carla/road/element/RoadObjectVisitor.h" #include "carla/road/InformationSet.h" #include "carla/road/general/Validity.h" #include "carla/road/signal/Signal.h" @@ -43,6 +45,11 @@ namespace road { info.first->_info = InformationSet(std::move(info.second)); } + for (auto &&info : _temp_road_object_container) { + DEBUG_ASSERT(info.first != nullptr); + info.first->_objects = ObjectSet(std::move(info.second)); + } + for (auto &&info : _temp_lane_info_container) { DEBUG_ASSERT(info.first != nullptr); info.first->_info = InformationSet(std::move(info.second)); @@ -50,6 +57,7 @@ namespace road { // remove temporal already used information _temp_road_info_container.clear(); + _temp_road_object_container.clear(); _temp_lane_info_container.clear(); // _map_data is a memeber of MapBuilder so you must especify if @@ -71,6 +79,24 @@ namespace road { _temp_road_info_container[road].emplace_back(std::move(elevation)); } + void MapBuilder::AddRoadObjectCrosswalk( + Road *road, + const std::string name, + const double s, + const double t, + const double zOffset, + const double hdg, + const double pitch, + const double roll, + const std::string orientation, + const double width, + const double length, + const std::vector points) { + DEBUG_ASSERT(road != nullptr); + auto cross = std::make_unique(s, name, t, zOffset, hdg, pitch, roll, std::move(orientation), width, length, std::move(points)); + _temp_road_object_container[road].emplace_back(std::move(cross)); + } + // called from lane parser void MapBuilder::CreateLaneAccess( Lane *lane, diff --git a/LibCarla/source/carla/road/MapBuilder.h b/LibCarla/source/carla/road/MapBuilder.h index 1bdea9eb3..8669004bd 100644 --- a/LibCarla/source/carla/road/MapBuilder.h +++ b/LibCarla/source/carla/road/MapBuilder.h @@ -7,6 +7,7 @@ #pragma once #include "carla/road/Map.h" +#include "carla/road/element/RoadObjectCrosswalk.h" #include @@ -108,6 +109,20 @@ namespace road { const double c, const double d); + void AddRoadObjectCrosswalk( + Road *road, + const std::string name, + const double s, + const double t, + const double zOffset, + const double hdg, + const double pitch, + const double roll, + const std::string orientation, + const double width, + const double length, + const std::vector points); + // void AddRoadLateralSuperElevation( // Road* road, // const double s, @@ -354,6 +369,9 @@ namespace road { std::unordered_map>> _temp_road_info_container; + std::unordered_map>> + _temp_road_object_container; + std::unordered_map>> _temp_lane_info_container; diff --git a/LibCarla/source/carla/road/ObjectSet.h b/LibCarla/source/carla/road/ObjectSet.h new file mode 100644 index 000000000..c7e7edfec --- /dev/null +++ b/LibCarla/source/carla/road/ObjectSet.h @@ -0,0 +1,53 @@ +// Copyright (c) 2019 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 . + +#pragma once + +#include "carla/NonCopyable.h" +#include "carla/road/RoadElementSet.h" +#include "carla/road/element/RoadObject.h" +#include "carla/road/element/RoadObjectIterator.h" + +#include +#include + +namespace carla { +namespace road { + + class ObjectSet : private MovableNonCopyable { + public: + + ObjectSet() = default; + + ObjectSet(std::vector> &&vec) + : _road_set(std::move(vec)) {} + + /// Return all objects given a type from the start of the road + template + std::vector GetObjects() const { + std::vector vec; + auto it = element::MakeRoadObjectIterator(_road_set.GetAll()); + for (; !it.IsAtEnd(); ++it) { + vec.emplace_back(&*it); + } + return vec; + } + + /// Returns single object given a type and a distance (s) from + /// the start of the road + template + const T *GetObject(const double s) const { + auto it = element::MakeRoadObjectIterator(_road_set.GetReverseSubset(s)); + return it.IsAtEnd() ? nullptr : &*it; + } + + private: + + RoadElementSet> _road_set; + }; + +} // road +} // carla diff --git a/LibCarla/source/carla/road/Road.h b/LibCarla/source/carla/road/Road.h index 3ac955291..6d75373ec 100644 --- a/LibCarla/source/carla/road/Road.h +++ b/LibCarla/source/carla/road/Road.h @@ -10,6 +10,7 @@ #include "carla/ListView.h" #include "carla/NonCopyable.h" #include "carla/road/InformationSet.h" +#include "carla/road/ObjectSet.h" #include "carla/road/Junction.h" #include "carla/road/LaneSection.h" #include "carla/road/LaneSectionMap.h" @@ -17,6 +18,7 @@ #include "carla/road/RoadTypes.h" #include "carla/road/element/Geometry.h" #include "carla/road/element/RoadInfo.h" +#include "carla/road/element/RoadObject.h" #include "carla/road/signal/Signal.h" #include "carla/road/signal/SignalReference.h" @@ -113,6 +115,16 @@ namespace road { return _info.GetInfo(s); } + template + const T *GetObject(const double s) const { + return _objects.GetObject(s); + } + + template + std::vector GetObjects() const { + return _objects.GetObjects(); + } + auto GetLaneSections() const { return MakeListView( iterator::make_map_values_const_iterator(_lane_sections.begin()), @@ -198,6 +210,9 @@ namespace road { std::unordered_map _signals; std::unordered_map _sign_ref; + + ObjectSet _objects; + }; } // road diff --git a/LibCarla/source/carla/road/element/RoadObject.h b/LibCarla/source/carla/road/element/RoadObject.h new file mode 100644 index 000000000..ca10a2e52 --- /dev/null +++ b/LibCarla/source/carla/road/element/RoadObject.h @@ -0,0 +1,43 @@ +// 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 . + +#pragma once + +#include "carla/road/element/RoadObjectVisitor.h" +#include "carla/NonCopyable.h" + +#include +#include +#include + +namespace carla { +namespace road { +namespace element { + + class RoadObject : private NonCopyable { + public: + + virtual ~RoadObject() = default; + + virtual void AcceptVisitor(RoadObjectVisitor &) = 0; + + /// Distance from road's start location. + double GetDistance() const { + return _s; + } + + protected: + + RoadObject(double distance = 0.0) : _s(distance) {} + + private: + + double _s; + }; + +} // namespace element +} // namespace road +} // namespace carla diff --git a/LibCarla/source/carla/road/element/RoadObjectCrosswalk.h b/LibCarla/source/carla/road/element/RoadObjectCrosswalk.h new file mode 100644 index 000000000..abd6e5dc3 --- /dev/null +++ b/LibCarla/source/carla/road/element/RoadObjectCrosswalk.h @@ -0,0 +1,79 @@ +// Copyright (c) 2019 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 . + +#pragma once + +#include "carla/road/element/RoadObject.h" + +namespace carla { +namespace road { +namespace element { + + struct CrosswalkPoint { + double u { 0.0 }; + double v { 0.0 }; + double z { 0.0 }; + CrosswalkPoint(double _u, double _v, double _z) : u(_u), v(_v), z(_z) {}; + }; + + class RoadObjectCrosswalk final : public RoadObject { + public: + + RoadObjectCrosswalk( + const double s, + const std::string name, + const double t, + const double zOffset, + const double hdg, + const double pitch, + const double roll, + const std::string orientation, + const double width, + const double length, + const std::vector points) + : RoadObject(s), + _name(name), + _t(t), + _zOffset(zOffset), + _hdg(hdg), + _pitch(pitch), + _roll(roll), + _orientation(orientation), + _width(width), + _length(length), + _points(points) {} + + void AcceptVisitor(RoadObjectVisitor &v) final { + v.Visit(*this); + } + + double GetS() const { return GetDistance(); }; + double GetT() const { return _t; }; + double GetWidth() const { return _width; }; + double GetLength() const { return _length; }; + double GetHeading() const { return _hdg; }; + double GetPitch() const { return _pitch; }; + double GetRoll() const { return _roll; }; + double GetZOffset() const { return _zOffset; }; + std::string GetOrientation() const { return _orientation; }; + const std::vector &GetPoints() const { return _points; }; + + private: + std::string _name; + double _t; + double _zOffset; + double _hdg; + double _pitch; + double _roll; + std::string _orientation; + double _width; + double _length; + std::vector _points; + }; + +} // namespace element +} // namespace road +} // namespace carla diff --git a/LibCarla/source/carla/road/element/RoadObjectIterator.h b/LibCarla/source/carla/road/element/RoadObjectIterator.h new file mode 100644 index 000000000..01ff555e5 --- /dev/null +++ b/LibCarla/source/carla/road/element/RoadObjectIterator.h @@ -0,0 +1,105 @@ +// Copyright (c) 2019 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 . + +#pragma once + +#include "carla/Debug.h" +#include "carla/road/element/RoadObjectVisitor.h" + +#include +#include + +namespace carla { +namespace road { +namespace element { + + template + class RoadObjectIterator : private RoadObjectVisitor { + public: + + static_assert(std::is_same, typename IT::value_type>::value, "Not compatible."); + + using value_type = T; + using difference_type = typename IT::difference_type; + using pointer = T *; + using reference = T &; + + RoadObjectIterator(IT begin, IT end) + : _it(begin), + _end(end) { + _success = false; + for (; !IsAtEnd(); ++_it) { + DEBUG_ASSERT((*_it) != nullptr); + (*_it)->AcceptVisitor(*this); + if (_success) { + break; + } + } + } + + RoadObjectIterator &operator++() { + _success = false; + while (!_success) { + ++_it; + if (IsAtEnd()) { + break; + } + DEBUG_ASSERT((*_it) != nullptr); + (*_it)->AcceptVisitor(*this); + } + return *this; + } + + reference operator*() const { + DEBUG_ASSERT((*_it) != nullptr); + return static_cast(**_it); + } + + pointer operator->() const { + DEBUG_ASSERT((*_it) != nullptr); + return static_cast(_it->get()); + } + + bool operator!=(const RoadObjectIterator &rhs) const { + return _it != rhs._it; + } + + bool operator==(const RoadObjectIterator &rhs) const { + return !((*this) != rhs); + } + + bool IsAtEnd() const { + return _it == _end; + } + + private: + + void Visit(T &) { + _success = true; + } + + IT _it; + + IT _end; + + bool _success; + }; + + template + static auto MakeRoadObjectIterator(const Container &c) { + auto begin = std::begin(c); + auto end = std::end(c); + return RoadObjectIterator(begin, end); + } + + template + static auto MakeRoadObjectIterator(IT begin, IT end) { + return RoadObjectIterator(begin, end); + } + +} // namespace element +} // namespace road +} // namespace carla diff --git a/LibCarla/source/carla/road/element/RoadObjectVisitor.h b/LibCarla/source/carla/road/element/RoadObjectVisitor.h new file mode 100644 index 000000000..ea3cc555e --- /dev/null +++ b/LibCarla/source/carla/road/element/RoadObjectVisitor.h @@ -0,0 +1,33 @@ +// 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 . + +#pragma once + +namespace carla { +namespace road { +namespace element { + + class RoadObject; + class RoadObjectCrosswalk; + + class RoadObjectVisitor { + public: + + RoadObjectVisitor() = default; + virtual ~RoadObjectVisitor() = default; + + RoadObjectVisitor(const RoadObjectVisitor &) = default; + RoadObjectVisitor(RoadObjectVisitor &&) = default; + + RoadObjectVisitor &operator=(const RoadObjectVisitor &) = default; + RoadObjectVisitor &operator=(RoadObjectVisitor &&) = default; + + virtual void Visit(RoadObjectCrosswalk &) {} + }; + +} // namespace element +} // namespace road +} // namespace carla diff --git a/PythonAPI/carla/source/libcarla/Map.cpp b/PythonAPI/carla/source/libcarla/Map.cpp index a5a0d6196..0327dd290 100644 --- a/PythonAPI/carla/source/libcarla/Map.cpp +++ b/PythonAPI/carla/source/libcarla/Map.cpp @@ -135,6 +135,7 @@ void export_map() { .def("transform_to_geolocation", &ToGeolocation, (arg("location"))) .def("to_opendrive", CALL_RETURNING_COPY(cc::Map, GetOpenDrive)) .def("save_to_disk", &SaveOpenDriveToDisk, (arg("path")="")) + .def("get_crosswalks", CALL_RETURNING_LIST(cc::Map, GetAllCrosswalkZones)) .def(self_ns::str(self_ns::self)) ;