primitives/Lanelet.h

Go to the documentation of this file.

// this is for emacs file handling -*- mode: c++; c-basic-offset: 2;
// indent-tabs-mode: nil -*-

#pragma once

#include <functional>
#include <memory>
#include <utility>

#include "lanelet2_core/Forward.h"
#include "lanelet2_core/primitives/LineString.h"
#include "lanelet2_core/primitives/Primitive.h"
#include "lanelet2_core/utility/Optional.h"

namespace lanelet {
enum class LaneletType { OneWay, Bidirectional };

class LaneletData : public PrimitiveData {
 public:
  LaneletData(Id id, LineString3d leftBound, LineString3d rightBound, const AttributeMap& attributes = AttributeMap(),
              RegulatoryElementPtrs regulatoryElements = RegulatoryElementPtrs())
      : PrimitiveData(id, attributes),
        leftBound_{std::move(leftBound)},
        rightBound_{std::move(rightBound)},
        regulatoryElements_{std::move(regulatoryElements)} {}

  const ConstLineString3d& leftBound() const { return leftBound_; }
  const LineString3d& leftBound() { return leftBound_; }

  const ConstLineString3d& rightBound() const { return rightBound_; }
  const LineString3d& rightBound() { return rightBound_; }

  RegulatoryElementConstPtrs regulatoryElements() const {
    return utils::transform(regulatoryElements_, [](const auto& elem) { return RegulatoryElementConstPtr(elem); });
  }
  RegulatoryElementPtrs& regulatoryElements() { return regulatoryElements_; }

  template <typename RegElemT>
  std::vector<std::shared_ptr<const RegElemT>> regulatoryElementsAs() const {
    return utils::transformSharedPtr<const RegElemT>(regulatoryElements_);
  }
  template <typename RegElemT>
  std::vector<std::shared_ptr<RegElemT>> regulatoryElementsAs() {
    return utils::transformSharedPtr<RegElemT>(regulatoryElements_);
  }

  void setLeftBound(const LineString3d& bound);

  void setRightBound(const LineString3d& bound);

  void setCenterline(const LineString3d& centerline);

  bool hasCustomCenterline() const;

  void resetCache() const;

  ConstLineString3d centerline() const;

  CompoundPolygon3d polygon() const;

 private:
  LineString3d leftBound_;                    
  LineString3d rightBound_;                   
  RegulatoryElementPtrs regulatoryElements_;  

  // Cached data
  mutable std::shared_ptr<ConstLineString3d> centerline_;
};

class ConstLanelet : public ConstPrimitive<LaneletData> {
 public:
  using ConstType = ConstLanelet;
  using MutableType = Lanelet;
  using TwoDType = ConstLanelet;
  using ThreeDType = ConstLanelet;
  using Category = traits::LaneletTag;

  explicit ConstLanelet(Id id = InvalId)
      : ConstLanelet(std::make_shared<LaneletData>(id, LineString3d(), LineString3d()), false) {}

  ConstLanelet(Id id, const LineString3d& leftBound, const LineString3d& rightBound,
               const AttributeMap& attributes = AttributeMap(),
               const RegulatoryElementPtrs& regulatoryElements = RegulatoryElementPtrs())
      : ConstPrimitive{std::make_shared<LaneletData>(id, leftBound, rightBound, attributes, regulatoryElements)} {}

  explicit ConstLanelet(const std::shared_ptr<const LaneletData>& data, bool inverted = false)
      : ConstPrimitive<lanelet::LaneletData>(data), inverted_{inverted} {}

  bool inverted() const { return inverted_; }

  ConstLanelet invert() const { return ConstLanelet{constData(), !inverted()}; }

  ConstLineString3d leftBound() const { return leftBound3d(); }
  ConstLineString2d leftBound2d() const { return utils::to2D(leftBound3d()); }
  ConstLineString3d leftBound3d() const {
    return inverted() ? constData()->rightBound().invert() : constData()->leftBound();
  }

  ConstLineString3d rightBound() const { return rightBound3d(); }
  ConstLineString2d rightBound2d() const { return utils::to2D(rightBound3d()); }
  ConstLineString3d rightBound3d() const {
    return inverted() ? constData()->leftBound().invert() : constData()->rightBound();
  }

  RegulatoryElementConstPtrs regulatoryElements() const { return constData()->regulatoryElements(); }

  template <typename RegElemT>
  std::vector<std::shared_ptr<const RegElemT>> regulatoryElementsAs() const {
    return constData()->regulatoryElementsAs<RegElemT>();
  }

  ConstLineString3d centerline() const { return centerline3d(); }

  ConstLineString2d centerline2d() const { return utils::to2D(centerline3d()); }

  ConstLineString3d centerline3d() const {
    return inverted() ? constData()->centerline().invert() : constData()->centerline();
  }

  bool hasCustomCenterline() const { return constData()->hasCustomCenterline(); }

  CompoundPolygon3d polygon3d() const;

  CompoundPolygon2d polygon2d() const;

  void resetCache() const { constData()->resetCache(); }

 private:
  bool inverted_{false};  
};

class Lanelet : public Primitive<ConstLanelet> {
 public:
  using Primitive::Primitive;
  using TwoDType = Lanelet;
  using ThreeDType = Lanelet;
  friend class ConstWeakLanelet;
  Lanelet() = default;
  Lanelet(const std::shared_ptr<const LaneletData>&, bool inverted) = delete;
  Lanelet(const LaneletDataPtr& data, bool inverted) : Primitive{data, inverted} {}

  Lanelet invert() const { return Lanelet{data(), !inverted()}; }

  using Primitive::leftBound;
  using Primitive::leftBound2d;
  using Primitive::leftBound3d;
  LineString3d leftBound() { return leftBound3d(); }

  LineString2d leftBound2d() { return utils::to2D(leftBound3d()); }

  LineString3d leftBound3d() { return inverted() ? data()->rightBound().invert() : data()->leftBound(); }

  using Primitive::rightBound;
  using Primitive::rightBound2d;
  using Primitive::rightBound3d;
  LineString3d rightBound() { return rightBound3d(); }

  LineString2d rightBound2d() { return utils::to2D(rightBound3d()); }

  LineString3d rightBound3d() { return inverted() ? data()->leftBound().invert() : data()->rightBound(); }

  ConstLineString3d rightBound() const { return rightBound3d(); }

  void setLeftBound(const LineString3d& bound) {
    inverted() ? data()->setRightBound(bound.invert()) : data()->setLeftBound(bound);
  }

  void setRightBound(const LineString3d& bound) {
    inverted() ? data()->setLeftBound(bound.invert()) : data()->setRightBound(bound);
  }

  void setCenterline(const LineString3d& centerline) {
    inverted() ? data()->setCenterline(centerline.invert()) : data()->setCenterline(centerline);
  }

  const RegulatoryElementPtrs& regulatoryElements() { return data()->regulatoryElements(); }

  void addRegulatoryElement(RegulatoryElementPtr regElem) {
    if (regElem == nullptr) {
      throw NullptrError("regulatory element is a nullptr.");
    }
    data()->regulatoryElements().push_back(std::move(regElem));
  }

  bool removeRegulatoryElement(const RegulatoryElementPtr& regElem) {
    auto& regElems = data()->regulatoryElements();
    auto remove = std::find(regElems.begin(), regElems.end(), regElem);
    if (remove != regElems.end()) {
      regElems.erase(remove);
      return true;
    }
    return false;
  }

  using Primitive::regulatoryElements;
  using Primitive::regulatoryElementsAs;

  template <typename RegElemT>
  std::vector<std::shared_ptr<RegElemT>> regulatoryElementsAs() {
    return data()->regulatoryElementsAs<RegElemT>();
  }
};

class ConstWeakLanelet {
 public:
  using ConstType = ConstWeakLanelet;
  using MutableType = WeakLanelet;
  using TwoDType = ConstWeakLanelet;
  using ThreeDType = ConstWeakLanelet;
  ConstWeakLanelet() = default;
  ConstWeakLanelet(const ConstLanelet& llet)  // NOLINT
      : laneletData_{llet.constData()}, inverted_{llet.inverted()} {}

  ConstLanelet lock() const { return ConstLanelet(laneletData_.lock(), inverted_); }

  bool expired() const noexcept { return laneletData_.expired(); }

 protected:
  std::weak_ptr<const LaneletData> laneletData_;  // NOLINT
  bool inverted_{false};                          // NOLINT
};

class WeakLanelet final : public ConstWeakLanelet {
 public:
  WeakLanelet() = default;
  WeakLanelet(const Lanelet& llet) : ConstWeakLanelet(llet) {}  // NOLINT

  Lanelet lock() const { return Lanelet(std::const_pointer_cast<LaneletData>(laneletData_.lock()), inverted_); }
};

namespace utils {
bool has(const ConstLanelet& ll, Id id);
}  // namespace utils

inline bool operator==(const ConstLanelet& lhs, const ConstLanelet& rhs) {
  return lhs.constData() == rhs.constData() && lhs.inverted() == rhs.inverted();
}
inline bool operator!=(const ConstLanelet& lhs, const ConstLanelet& rhs) { return !(lhs == rhs); }
inline bool operator==(const ConstWeakLanelet& lhs, const ConstWeakLanelet& rhs) {
  return !lhs.expired() && !rhs.expired() && lhs.lock() == rhs.lock();
}
inline bool operator!=(const ConstWeakLanelet& lhs, const ConstWeakLanelet& rhs) { return !(lhs == rhs); }
std::ostream& operator<<(std::ostream& stream, const ConstLanelet& obj);

namespace traits {
template <typename T>
constexpr bool isLaneletT() {
  return isCategory<T, traits::LaneletTag>();
}
}  // namespace traits

template <typename T, typename RetT>
using IfLL = std::enable_if_t<traits::isLaneletT<T>(), RetT>;

}  // namespace lanelet

// Hash function for usage in containers
namespace std {
template <>
struct hash<lanelet::Lanelet> : public lanelet::HashBase<lanelet::Lanelet> {};
template <>
struct hash<lanelet::ConstLanelet> : public lanelet::HashBase<lanelet::ConstLanelet> {};

template <typename T, typename U>
struct hash<std::pair<U, T>> {
  size_t operator()(const std::pair<U, T> x) const noexcept {
    return std::hash<U>()(x.first) ^ std::hash<T>()(x.second);
  }
};

}  // namespace std