126 lines
4.2 KiB
C++
126 lines
4.2 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 <chrono>
|
|
#include <memory>
|
|
#include <unordered_map>
|
|
#include <unordered_set>
|
|
|
|
#if defined(__clang__)
|
|
# pragma clang diagnostic push
|
|
# pragma clang diagnostic ignored "-Wshadow"
|
|
#endif
|
|
#include "boost/geometry.hpp"
|
|
#include "boost/geometry/geometries/point.hpp"
|
|
#include "boost/geometry/index/rtree.hpp"
|
|
#if defined(__clang__)
|
|
# pragma clang diagnostic pop
|
|
#endif
|
|
|
|
#include "carla/client/Map.h"
|
|
#include "carla/client/Waypoint.h"
|
|
#include "carla/geom/Location.h"
|
|
#include "carla/geom/Math.h"
|
|
#include "carla/Memory.h"
|
|
#include "carla/road/RoadTypes.h"
|
|
|
|
#include "carla/trafficmanager/RandomGenerator.h"
|
|
#include "carla/trafficmanager/SimpleWaypoint.h"
|
|
#include "carla/trafficmanager/CachedSimpleWaypoint.h"
|
|
|
|
namespace carla {
|
|
namespace traffic_manager {
|
|
|
|
namespace cg = carla::geom;
|
|
namespace cc = carla::client;
|
|
namespace crd = carla::road;
|
|
namespace bg = boost::geometry;
|
|
namespace bgi = boost::geometry::index;
|
|
|
|
using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
|
|
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
|
|
using NodeList = std::vector<SimpleWaypointPtr>;
|
|
using GeoGridId = crd::JuncId;
|
|
using WorldMap = carla::SharedPtr<const cc::Map>;
|
|
|
|
using Point3D = bg::model::point<float, 3, bg::cs::cartesian>;
|
|
using Box = bg::model::box<Point3D>;
|
|
using SpatialTreeEntry = std::pair<Point3D, SimpleWaypointPtr>;
|
|
|
|
using SegmentId = std::tuple<crd::RoadId, crd::LaneId, crd::SectionId>;
|
|
using SegmentTopology = std::map<SegmentId, std::pair<std::vector<SegmentId>, std::vector<SegmentId>>>;
|
|
using SegmentMap = std::map<SegmentId, std::vector<SimpleWaypointPtr>>;
|
|
using Rtree = bgi::rtree<SpatialTreeEntry, bgi::rstar<16>>;
|
|
|
|
/// This class builds a discretized local map-cache.
|
|
/// Instantiate the class with the world and run SetUp() to construct the
|
|
/// local map.
|
|
class InMemoryMap {
|
|
|
|
private:
|
|
|
|
/// Object to hold the world map received by the constructor.
|
|
WorldMap _world_map;
|
|
/// Structure to hold all custom waypoint objects after interpolation of
|
|
/// sparse topology.
|
|
NodeList dense_topology;
|
|
/// Spatial quadratic R-tree for indexing and querying waypoints.
|
|
Rtree rtree;
|
|
|
|
public:
|
|
|
|
InMemoryMap(WorldMap world_map);
|
|
~InMemoryMap();
|
|
|
|
static void Cook(WorldMap world_map, const std::string& path);
|
|
|
|
//bool Load(const std::string& filename);
|
|
bool Load(const std::vector<uint8_t>& content);
|
|
|
|
/// This method constructs the local map with a resolution of sampling_resolution.
|
|
void SetUp();
|
|
|
|
/// This method returns the closest waypoint to a given location on the map.
|
|
SimpleWaypointPtr GetWaypoint(const cg::Location loc) const;
|
|
|
|
/// This method returns n waypoints in an delta area with a certain distance from the ego vehicle.
|
|
NodeList GetWaypointsInDelta(const cg::Location loc, const uint16_t n_points, const float random_sample) const;
|
|
|
|
/// This method returns the full list of discrete samples of the map in the local cache.
|
|
NodeList GetDenseTopology() const;
|
|
|
|
std::string GetMapName();
|
|
|
|
const cc::Map& GetMap() const;
|
|
|
|
private:
|
|
void Save(const std::string& path);
|
|
|
|
void SetUpDenseTopology();
|
|
void SetUpSpatialTree();
|
|
void SetUpRoadOption();
|
|
|
|
/// This method is used to find and place lane change links.
|
|
void FindAndLinkLaneChange(SimpleWaypointPtr reference_waypoint);
|
|
|
|
NodeList GetSuccessors(const SegmentId segment_id,
|
|
const SegmentTopology &segment_topology,
|
|
const SegmentMap &segment_map);
|
|
NodeList GetPredecessors(const SegmentId segment_id,
|
|
const SegmentTopology &segment_topology,
|
|
const SegmentMap &segment_map);
|
|
|
|
/// Computes the segment id of a given waypoint.
|
|
/// The Id takes into account OpenDrive's road Id, lane Id and Section Id.
|
|
SegmentId GetSegmentId(const WaypointPtr &wp) const;
|
|
SegmentId GetSegmentId(const SimpleWaypointPtr &swp) const;
|
|
};
|
|
|
|
} // namespace traffic_manager
|
|
} // namespace carla
|