263 lines
10 KiB
C++
263 lines
10 KiB
C++
// Copyright (c) 2020 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/Mesh.h"
|
|
#include "carla/geom/Rtree.h"
|
|
#include "carla/geom/Transform.h"
|
|
#include "carla/NonCopyable.h"
|
|
#include "carla/road/element/LaneMarking.h"
|
|
#include "carla/road/element/RoadInfoMarkRecord.h"
|
|
#include "carla/road/element/Waypoint.h"
|
|
#include "carla/road/MapData.h"
|
|
#include "carla/road/RoadTypes.h"
|
|
#include "carla/road/MeshFactory.h"
|
|
#include "carla/geom/Vector3D.h"
|
|
#include "carla/rpc/OpendriveGenerationParameters.h"
|
|
|
|
#include <boost/optional.hpp>
|
|
|
|
#include <vector>
|
|
|
|
namespace carla {
|
|
namespace road {
|
|
|
|
class Map : private MovableNonCopyable {
|
|
public:
|
|
|
|
using Waypoint = element::Waypoint;
|
|
|
|
/// ========================================================================
|
|
/// -- Constructor ---------------------------------------------------------
|
|
/// ========================================================================
|
|
|
|
Map(MapData m) : _data(std::move(m)) {
|
|
CreateRtree();
|
|
}
|
|
|
|
/// ========================================================================
|
|
/// -- Georeference --------------------------------------------------------
|
|
/// ========================================================================
|
|
|
|
const geom::GeoLocation &GetGeoReference() const {
|
|
return _data.GetGeoReference();
|
|
}
|
|
|
|
/// ========================================================================
|
|
/// -- Geometry ------------------------------------------------------------
|
|
/// ========================================================================
|
|
|
|
boost::optional<element::Waypoint> GetClosestWaypointOnRoad(
|
|
const geom::Location &location,
|
|
int32_t lane_type = static_cast<int32_t>(Lane::LaneType::Driving)) const;
|
|
|
|
boost::optional<element::Waypoint> GetWaypoint(
|
|
const geom::Location &location,
|
|
int32_t lane_type = static_cast<int32_t>(Lane::LaneType::Driving)) const;
|
|
|
|
boost::optional<element::Waypoint> GetWaypoint(
|
|
RoadId road_id,
|
|
LaneId lane_id,
|
|
float s) const;
|
|
|
|
geom::Transform ComputeTransform(Waypoint waypoint) const;
|
|
|
|
/// ========================================================================
|
|
/// -- Road information ----------------------------------------------------
|
|
/// ========================================================================
|
|
|
|
const Lane &GetLane(Waypoint waypoint) const;
|
|
|
|
Lane::LaneType GetLaneType(Waypoint waypoint) const;
|
|
|
|
double GetLaneWidth(Waypoint waypoint) const;
|
|
|
|
JuncId GetJunctionId(RoadId road_id) const;
|
|
|
|
bool IsJunction(RoadId road_id) const;
|
|
|
|
std::pair<const element::RoadInfoMarkRecord *, const element::RoadInfoMarkRecord *>
|
|
GetMarkRecord(Waypoint waypoint) const;
|
|
|
|
std::vector<element::LaneMarking> CalculateCrossedLanes(
|
|
const geom::Location &origin,
|
|
const geom::Location &destination) const;
|
|
|
|
/// Returns a list of locations defining 2d areas,
|
|
/// when a location is repeated an area is finished
|
|
std::vector<geom::Location> GetAllCrosswalkZones() const;
|
|
|
|
/// Data structure for the signal search
|
|
struct SignalSearchData {
|
|
const element::RoadInfoSignal *signal;
|
|
Waypoint waypoint;
|
|
double accumulated_s = 0;
|
|
};
|
|
|
|
/// Searches signals from an initial waypoint until the defined distance.
|
|
std::vector<SignalSearchData> GetSignalsInDistance(
|
|
Waypoint waypoint, double distance, bool stop_at_junction = false) const;
|
|
|
|
/// Return all RoadInfoSignal in the map
|
|
std::vector<const element::RoadInfoSignal*>
|
|
GetAllSignalReferences() const;
|
|
|
|
/// ========================================================================
|
|
/// -- Waypoint generation -------------------------------------------------
|
|
/// ========================================================================
|
|
|
|
/// Return the list of waypoints placed at the entrance of each drivable
|
|
/// successor lane; i.e., the list of each waypoint in the next road segment
|
|
/// that a vehicle could drive from @a waypoint.
|
|
std::vector<Waypoint> GetSuccessors(Waypoint waypoint) const;
|
|
std::vector<Waypoint> GetPredecessors(Waypoint waypoint) const;
|
|
|
|
/// Return the list of waypoints at @a distance such that a vehicle at @a
|
|
/// waypoint could drive to.
|
|
std::vector<Waypoint> GetNext(Waypoint waypoint, double distance) const;
|
|
/// Return the list of waypoints at @a distance in the reversed direction
|
|
/// that a vehicle at @a waypoint could drive to.
|
|
std::vector<Waypoint> GetPrevious(Waypoint waypoint, double distance) const;
|
|
|
|
/// Return a waypoint at the lane of @a waypoint's right lane.
|
|
boost::optional<Waypoint> GetRight(Waypoint waypoint) const;
|
|
|
|
/// Return a waypoint at the lane of @a waypoint's left lane.
|
|
boost::optional<Waypoint> GetLeft(Waypoint waypoint) const;
|
|
|
|
/// Generate all the waypoints in @a map separated by @a approx_distance.
|
|
std::vector<Waypoint> GenerateWaypoints(double approx_distance) const;
|
|
|
|
/// Generate waypoints on each @a lane at the start of each @a road
|
|
std::vector<Waypoint> GenerateWaypointsOnRoadEntries(Lane::LaneType lane_type = Lane::LaneType::Driving) const;
|
|
|
|
/// Generate waypoints at the entry of each lane of the specified road
|
|
std::vector<Waypoint> GenerateWaypointsInRoad(RoadId road_id, Lane::LaneType lane_type = Lane::LaneType::Driving) const;
|
|
|
|
/// Generate the minimum set of waypoints that define the topology of @a
|
|
/// map. The waypoints are placed at the entrance of each lane.
|
|
std::vector<std::pair<Waypoint, Waypoint>> GenerateTopology() const;
|
|
|
|
/// Generate waypoints of the junction
|
|
std::vector<std::pair<Waypoint, Waypoint>> GetJunctionWaypoints(JuncId id, Lane::LaneType lane_type) const;
|
|
|
|
Junction* GetJunction(JuncId id);
|
|
|
|
const Junction* GetJunction(JuncId id) const;
|
|
|
|
std::unordered_map<road::RoadId, std::unordered_set<road::RoadId>>
|
|
ComputeJunctionConflicts(JuncId id) const;
|
|
|
|
/// Buids a mesh based on the OpenDRIVE
|
|
geom::Mesh GenerateMesh(
|
|
const double distance,
|
|
const float extra_width = 0.6f,
|
|
const bool smooth_junctions = true) const;
|
|
|
|
std::vector<std::unique_ptr<geom::Mesh>> GenerateChunkedMesh(
|
|
const rpc::OpendriveGenerationParameters& params) const;
|
|
|
|
std::map<road::Lane::LaneType , std::vector<std::unique_ptr<geom::Mesh>>>
|
|
GenerateOrderedChunkedMeshInLocations( const rpc::OpendriveGenerationParameters& params,
|
|
const geom::Vector3D& minpos,
|
|
const geom::Vector3D& maxpos) const;
|
|
|
|
/// Buids a mesh of all crosswalks based on the OpenDRIVE
|
|
geom::Mesh GetAllCrosswalkMesh() const;
|
|
|
|
std::vector<std::pair<geom::Transform, std::string>> GetTreesTransform(
|
|
const geom::Vector3D& minpos,
|
|
const geom::Vector3D& maxpos,
|
|
float distancebetweentrees,
|
|
float distancefromdrivinglineborder,
|
|
float s_offset = 0) const;
|
|
|
|
geom::Mesh GenerateWalls(const double distance, const float wall_height) const;
|
|
|
|
/// Buids a list of meshes related with LineMarkings
|
|
std::vector<std::unique_ptr<geom::Mesh>> GenerateLineMarkings(
|
|
const rpc::OpendriveGenerationParameters& params,
|
|
const geom::Vector3D& minpos,
|
|
const geom::Vector3D& maxpos,
|
|
std::vector<std::string>& outinfo ) const;
|
|
|
|
const std::unordered_map<SignId, std::unique_ptr<Signal>>& GetSignals() const {
|
|
return _data.GetSignals();
|
|
}
|
|
|
|
const std::unordered_map<ContId, std::unique_ptr<Controller>>& GetControllers() const {
|
|
return _data.GetControllers();
|
|
}
|
|
|
|
std::vector<carla::geom::BoundingBox> GetJunctionsBoundingBoxes() const;
|
|
|
|
#ifdef LIBCARLA_WITH_GTEST
|
|
MapData &GetMap() {
|
|
return _data;
|
|
}
|
|
#endif // LIBCARLA_WITH_GTEST
|
|
|
|
private:
|
|
|
|
friend MapBuilder;
|
|
MapData _data;
|
|
|
|
using Rtree = geom::SegmentCloudRtree<Waypoint>;
|
|
Rtree _rtree;
|
|
|
|
void CreateRtree();
|
|
|
|
/// Helper Functions for constructing the rtree element list
|
|
void AddElementToRtree(
|
|
std::vector<Rtree::TreeElement> &rtree_elements,
|
|
geom::Transform ¤t_transform,
|
|
geom::Transform &next_transform,
|
|
Waypoint ¤t_waypoint,
|
|
Waypoint &next_waypoint);
|
|
|
|
void AddElementToRtreeAndUpdateTransforms(
|
|
std::vector<Rtree::TreeElement> &rtree_elements,
|
|
geom::Transform ¤t_transform,
|
|
Waypoint ¤t_waypoint,
|
|
Waypoint &next_waypoint);
|
|
|
|
public:
|
|
inline float GetZPosInDeformation(float posx, float posy) const;
|
|
|
|
std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>>
|
|
GenerateRoadsMultithreaded( const carla::geom::MeshFactory& mesh_factory,
|
|
const std::vector<RoadId>& RoadsID,
|
|
const size_t index,
|
|
const size_t number_of_roads_per_thread) const;
|
|
|
|
void GenerateJunctions(const carla::geom::MeshFactory& mesh_factory,
|
|
const rpc::OpendriveGenerationParameters& params,
|
|
const geom::Vector3D& minpos,
|
|
const geom::Vector3D& maxpos,
|
|
std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>>*
|
|
juntion_out_mesh_list) const;
|
|
|
|
void GenerateSingleJunction(const carla::geom::MeshFactory& mesh_factory,
|
|
const JuncId Id,
|
|
std::map<road::Lane::LaneType, std::vector<std::unique_ptr<geom::Mesh>>>*
|
|
junction_out_mesh_list) const;
|
|
|
|
// Return list of junction ID which are between those positions
|
|
std::vector<JuncId> FilterJunctionsByPosition(
|
|
const geom::Vector3D& minpos,
|
|
const geom::Vector3D& maxpos) const;
|
|
// Return list of roads ID which are between those positions
|
|
std::vector<RoadId> FilterRoadsByPosition(
|
|
const geom::Vector3D& minpos,
|
|
const geom::Vector3D& maxpos ) const;
|
|
|
|
std::unique_ptr<geom::Mesh> SDFToMesh(const road::Junction& jinput, const std::vector<geom::Vector3D>& sdfinput, int grid_cells_per_dim) const;
|
|
};
|
|
|
|
} // namespace road
|
|
} // namespace carla
|