RegulatoryElement.cpp

Go to the documentation of this file.

#include "lanelet2_core/primitives/RegulatoryElement.h"
 
#include "lanelet2_core/primitives/Area.h"
#include "lanelet2_core/primitives/BasicRegulatoryElements.h"
#include "lanelet2_core/primitives/Lanelet.h"
#include "lanelet2_core/primitives/LineString.h"
#include "lanelet2_core/primitives/Point.h"
 
namespace lanelet {
namespace {
class HasIdVisitor : public RuleParameterVisitor {
 public:
  explicit HasIdVisitor(Id id) : id_{id} {}
  void operator()(const ConstPoint3d& p) override { found_ |= p.id() == id_; }
  void operator()(const ConstLineString3d& l) override { found_ |= l.id() == id_ || utils::has(l, id_); }
  void operator()(const ConstPolygon3d& p) override { found_ |= p.id() == id_ || utils::has(p, id_); }
  void operator()(const ConstWeakLanelet& ll) override {
    if (ll.expired()) {
      return;
    }
    ConstLanelet llet(ll.lock());
    found_ |= llet.id() == id_ || utils::has(llet, id_);
  }
  void operator()(const ConstWeakArea& ar) override {
    if (ar.expired()) {
      return;
    }
    ConstArea area(ar.lock());
    found_ |= area.id() == id_ || utils::has(area, id_);
  }
  bool operator!() const { return !found_; }
 
 private:
  Id id_;
  bool found_{false};
};
 
class GetIdVisitor : public RuleParameterVisitor {
 public:
  static Id id(const ConstRuleParameter& param) {
    GetIdVisitor visitor;
    boost::apply_visitor(visitor, param);
    return visitor.id_;
  }
  template <typename PrimT>
  void appendID(const PrimT& p) {
    id_ = p.id();
  }
 
  void operator()(const ConstPoint3d& p) override { appendID(p); }
  void operator()(const ConstLineString3d& l) override { appendID(l); }
  void operator()(const ConstPolygon3d& p) override { appendID(p); }
  void operator()(const ConstWeakLanelet& ll) override {
    if (!ll.expired()) {
      appendID(ll.lock());
    }
  }
  void operator()(const ConstWeakArea& ar) override {
    if (!ar.expired()) {
      appendID(ar.lock());
    }
  }
 
 private:
  Id id_{};
};
 
class ToConstVisitor : public RuleParameterVisitor {
 public:
  void operator()(const ConstPoint3d& p) override { param_ = p; }
  void operator()(const ConstLineString3d& l) override { param_ = l; }
  void operator()(const ConstPolygon3d& p) override { param_ = p; }
  void operator()(const ConstWeakLanelet& ll) override { param_ = ll; }
  void operator()(const ConstWeakArea& ar) override { param_ = ar; }
 
  ConstRuleParameter apply(const RuleParameter& rule) {
    boost::apply_visitor(*this, rule);
    return param_;
  }
 
 private:
  ConstRuleParameter param_;
};
 
}  // namespace
 
static RegisterRegulatoryElement<GenericRegulatoryElement> genRegelem;
#if __cplusplus < 201703L
constexpr char GenericRegulatoryElement::RuleName[];
 
constexpr const char RoleNameString::Refers[];
constexpr const char RoleNameString::RefLine[];
constexpr const char RoleNameString::Yield[];
constexpr const char RoleNameString::RightOfWay[];
constexpr const char RoleNameString::Cancels[];
constexpr const char RoleNameString::CancelLine[];
 
// roles not included in fast lookup
constexpr const char RoleNameString::Left[];
constexpr const char RoleNameString::Right[];
constexpr const char RoleNameString::Centerline[];
constexpr const char RoleNameString::Inner[];
constexpr const char RoleNameString::Outer[];
constexpr const char RoleNameString::Lanelet[];
constexpr const char RoleNameString::RegulatoryElement[];
 
constexpr char RegulatoryElement::RuleName[];
 
constexpr RoleNameString::RoleNamesItem RoleNameString::Map[];
#endif
 
RegulatoryElement::~RegulatoryElement() = default;
 
RegulatoryElementPtr RegulatoryElementFactory::create(std::string ruleName, const RegulatoryElementDataPtr& data) {
  if (ruleName.empty()) {
    ruleName = GenericRegulatoryElement::RuleName;
  }
  data->attributes[AttributeNamesString::Subtype] = ruleName;
  auto& inst = RegulatoryElementFactory::instance();
  auto it = inst.registry_.find(ruleName);
  if (it != inst.registry_.end()) {
    return it->second(data);
  }
  throw InvalidInputError("No regulatory element found that implements rule " + ruleName);
}
 
std::vector<std::string> RegulatoryElementFactory::availableRules() {
  auto& registry = RegulatoryElementFactory::instance().registry_;
  return utils::transform(registry, [](const auto& elem) { return elem.first; });
}
 
RegulatoryElementFactory& RegulatoryElementFactory::instance() {
  static RegulatoryElementFactory factory;
  return factory;
}
 
ConstRuleParameterMap RegulatoryElement::getParameters() const {
  ConstRuleParameterMap params;
  for (const auto& param : parameters()) {
    params.insert(std::make_pair(
        param.first, utils::transform(param.second, [](const auto& elem) { return traits::toConst(elem); })));
  }
  return params;
}
 
void RegulatoryElement::applyVisitor(RuleParameterVisitor& visitor) const {
  for (const auto& elems : parameters()) {
    visitor.role = elems.first;
    for (const auto& elem : elems.second) {
      boost::apply_visitor(visitor, elem);
    }
  }
}
 
void RegulatoryElement::applyVisitor(lanelet::internal::MutableParameterVisitor& visitor) const {
  for (const auto& elems : parameters()) {
    visitor.role = elems.first;
    for (const auto& elem : elems.second) {
      boost::apply_visitor(visitor, elem);
    }
  }
}
 
bool utils::has(const RegulatoryElement& regElem, Id id) {
  HasIdVisitor hasId(id);
  regElem.applyVisitor(hasId);
  return !!hasId;
}
 
template <>
ConstRuleParameter traits::toConst<RuleParameter>(const RuleParameter& primitive) {
  return ToConstVisitor().apply(primitive);
}
 
std::ostream& operator<<(std::ostream& stream, const RegulatoryElement& obj) {
  stream << "[id: " << obj.id();
  if (!obj.empty()) {
    stream << ", parameters: ";
    for (auto& param : obj.getParameters()) {
      stream << '{' << param.first << ':' << ' ';
      for (auto& rule : param.second) {
        stream << GetIdVisitor::id(rule) << ' ';
      }
      stream << '}';
    }
  }
  return stream << ']';
}
 
template <>
Id traits::getId<RuleParameter>(const RuleParameter& prim) {
  return GetIdVisitor::id(prim);
}
 
template <>
Id traits::getId<ConstRuleParameter>(const ConstRuleParameter& prim) {
  return GetIdVisitor::id(prim);
}
 
}  // namespace lanelet