LaneletSequence.h

Go to the documentation of this file.

#pragma once
#include <utility>

#include "lanelet2_core/primitives/CompoundLineString.h"
#include "lanelet2_core/primitives/CompoundPolygon.h"
#include "lanelet2_core/primitives/Lanelet.h"

namespace lanelet {

class LaneletSequenceData : public boost::noncopyable {
 public:
  using iterator =  // NOLINT
      internal::ReverseAndForwardIterator<ConstLanelets::const_iterator>;
  explicit LaneletSequenceData(ConstLanelets lanelets);

  const CompoundLineString3d& leftBound() const { return leftBound3d(); }
  const CompoundLineString3d& leftBound3d() const { return leftBound_; }
  CompoundLineString2d leftBound2d() const { return utils::to2D(leftBound_); }

  const CompoundLineString3d& rightBound() const { return rightBound3d(); }
  const CompoundLineString3d& rightBound3d() const { return rightBound_; }
  CompoundLineString2d rightBound2d() const { return utils::to2D(rightBound3d()); }
  const ConstLanelets& lanelets() const { return lanelets_; }

  CompoundLineString3d centerline() const;

  CompoundPolygon3d polygon() const;

  iterator begin(bool inverted) const noexcept {
    return inverted ? iterator(lanelets_.rbegin()) : iterator(lanelets_.begin());
  }

  iterator end(bool inverted) const noexcept {
    return inverted ? iterator(lanelets_.rend()) : iterator(lanelets_.end());
  }

 private:
  const ConstLanelets lanelets_;  // NOLINT
  const CompoundLineString3d leftBound_;                      // NOLINT
  const CompoundLineString3d rightBound_;                     // NOLINT
  mutable std::shared_ptr<CompoundLineString3d> centerline_;  
  mutable std::shared_ptr<CompoundPolygon3d> polygon_;        
};

class LaneletSequence {
 public:
  using DataType = LaneletSequenceData;
  using TwoDType = void;
  using ThreeDType = void;
  using HybridType = void;
  using ConstType = LaneletSequence;
  using MutableType = void;
  using iterator = LaneletSequenceData::iterator;  // NOLINT

  LaneletSequence(ConstLanelets ls = ConstLanelets())  // NOLINT
      : data_{std::make_shared<LaneletSequenceData>(std::move(ls))} {}

  explicit LaneletSequence(const LaneletSequences& ls)
      : LaneletSequence(utils::concatenate(ls, [](const auto& e) { return e.lanelets(); })) {}

  LaneletSequence(LaneletSequenceDataConstPtr data, bool inverted) : data_{std::move(data)}, inverted_{inverted} {}

  bool inverted() const { return inverted_; }

  bool empty() const noexcept { return data_->lanelets().empty(); }

  size_t size() const noexcept { return data_->lanelets().size(); }

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

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

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

  RegulatoryElementConstPtrs regulatoryElements() const;

  template <typename RegElemT>
  std::vector<std::shared_ptr<const RegElemT>> regulatoryElementsAs() const {
    return utils::concatenate(lanelets(),
                              [](const auto& elem) { return elem.template regulatoryElementsAs<RegElemT>(); });
  }

  CompoundLineString3d centerline() const { return centerline3d(); }
  CompoundLineString3d centerline3d() const {
    return inverted() ? constData()->centerline().invert() : constData()->centerline();
  }
  CompoundLineString2d centerline2d() const { return utils::to2D(centerline3d()); }

  CompoundPolygon3d polygon3d() const { return constData()->polygon(); }

  CompoundPolygon2d polygon2d() const { return CompoundPolygon2d(polygon3d()); }

  ConstLanelets lanelets() const { return inverted() ? utils::invert(data_->lanelets()) : data_->lanelets(); }

  iterator begin() const noexcept { return data_->begin(inverted()); }

  iterator end() const noexcept { return data_->end(inverted()); }

  const ConstLanelet& operator[](size_t idx) const {
    assert(idx < data_->lanelets().size());
    return *std::next(begin(), idx);
  }

  Ids ids() const {
    return utils::transform(lanelets(), [](const auto& ls) { return ls.id(); });
  }

  LaneletSequenceDataConstPtr constData() const { return data_; }

  void resetCache() { data_ = std::make_shared<LaneletSequenceData>(data_->lanelets()); }

 private:
  std::shared_ptr<const LaneletSequenceData> data_;  
  bool inverted_{false};                             
};

namespace utils {
inline bool has(const LaneletSequence& ll, Id id) { return has(ll.leftBound(), id) || has(ll.rightBound(), id); }
}  // namespace utils

inline bool operator==(const LaneletSequence& lhs, const LaneletSequence& rhs) {
  return lhs.lanelets() == rhs.lanelets();
}
inline bool operator!=(const LaneletSequence& lhs, const LaneletSequence& rhs) { return !(rhs == lhs); }

inline std::ostream& operator<<(std::ostream& stream, const LaneletSequence& obj) {
  auto ids = obj.ids();
  stream << "[";
  if (!ids.empty()) {
    stream << "ids: ";
    std::copy(ids.begin(), ids.end(), std::ostream_iterator<Id>(stream, " "));
  }
  if (obj.inverted()) {
    stream << ", inverted";
  }
  return stream << "]";
}
}  // namespace lanelet