【c++】Lanelet2 Examples笔记（二）

最新推荐文章于 2023-06-08 08:48:28 发布

Zhang_Chen_

最新推荐文章于 2023-06-08 08:48:28 发布

阅读量2.4k

点赞数 1

分类专栏： C++ 文章标签： c++

本文链接：https://blog.csdn.net/Zhang_Chen_/article/details/121976716

版权

C++ 专栏收录该内容

9 篇文章 5 订阅

订阅专栏

【c++】Lanelet2 Examples笔记（二）

02_regulatory_elements
03_lanelet_map
结语

02_regulatory_elements

#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_core/primitives/BasicRegulatoryElements.h>
#include <lanelet2_core/primitives/Lanelet.h>

#include "lanelet2_examples/internal/ExampleHelpers.h"

// we want assert statements to work in release mode
#undef NDEBUG

void part1BasicRegulatoryElements();
void part2HandlingRegulatoryElements();
void part3AddingNewRegulatoryElements();

int main() {
  part1BasicRegulatoryElements();
  part2HandlingRegulatoryElements();
  part3AddingNewRegulatoryElements();
  return 0;
}

void part1BasicRegulatoryElements() {
  using namespace lanelet;
  // Lanelet2 定义了一些基本的监管元素，例如交通灯和限速。

  // 在lanelet2中使用LineString3d表示traffic lights，其type属性为TrafficLight
  LineString3d trafficLight = examples::getLineStringAtY(1);
  trafficLight.attributes()[AttributeName::Type] = AttributeValueString::TrafficLight;

  // this creates our traffic light. Regelems are passed around as shared pointers.
  RegulatoryElementPtr trafficLightRegelem = lanelet::TrafficLight::make(utils::getId(), {}, {trafficLight});

  // 通过.addRegulatoryElement()把交通信号灯加到一个Lanelet中
  Lanelet lanelet = examples::getALanelet();
  lanelet.addRegulatoryElement(trafficLightRegelem); 
  assert(lanelet.regulatoryElements().size() == 1);

  // 通过.regulatoryElements()查看lanelet上的交通监管信息，.regulatoryElements()返回一个vector
  RegulatoryElementPtr regelem = lanelet.regulatoryElements()[0];

  // 通过.regulatoryElementsAs<SpeedLimit>()查看一个Lanelet上的限速规则，<>中可以定义查看的具体规则类别
  assert(lanelet.regulatoryElementsAs<SpeedLimit>().empty()); 
  // .regulatoryElementsAs返回一个vector
  std::vector<TrafficLight::Ptr> trafficLightRegelems = lanelet.regulatoryElementsAs<TrafficLight>();
  assert(trafficLightRegelems.size() == 1);
  TrafficLight::Ptr tlRegelem = trafficLightRegelems.front();      
  assert(tlRegelem->constData() == trafficLightRegelem->constData());  

  // from traffic lights we can directly get the relevant lights and the stop line (we didnt set one, but we could).
  // since traffic lights can either be a polygon or a linestring, we get an object that represents both.
  LineStringOrPolygon3d theLight = tlRegelem->trafficLights().front();
  assert(theLight == trafficLight);
  
  // 可以修改
  // we can also modify it, and since regulatory element data is shared, this also affects the lanelet that holds it
  tlRegelem->setStopLine(examples::getLineStringAtY(2));
  assert(!!tlRegelem->stopLine());

  // there are much more regulatory elements, but they all work basically in the same way as shown here.
}

void part2HandlingRegulatoryElements() {
  using namespace lanelet;
  // GenericRegulatoryElementke可以是任何交通规则
  GenericRegulatoryElement regelem(utils::getId());
  
  // 任何primitive都可以被赋予交通规则，并通过.addParameter添加到GenericRegulatoryElement
  // to the generic regulatory elements we can add any primitive (point, linestring, lanelet, area) with any role:
  Lanelet lanelet = examples::getALanelet();
  regelem.addParameter(RoleName::Refers, lanelet);
  Point3d point(utils::getId(), 0, 0, 0);
  regelem.addParameter(RoleName::Refers, point);
   
  // 通过.getParameters<primitive类型>(规则类型)，得到GenericRegulatoryElementke中的被赋予指定交通规则的primitive
  // now two different primitives have been added with the same role. Internally they are stored as boost::variants.
  // to read them from the regelem, we have to pass the type we are looking for:
  Points3d pts = regelem.getParameters<Point3d>(RoleName::Refers);
  assert(!pts.empty() && pts.front() == point);

  // this interface could be used add more things with more nonsense role names. But that would be hard to interpret.
  // For that reason, the implementations of regulatory elements provide an interface that gives less opportunity for
  // abuse.
}

// as an example, we create a new regulatory element and register it with lanelet2. It is a "LightsOn" regulatory
// element, that tells the vehicle to turn its lights on after passing a specific line
namespace example {
class LightsOn : public lanelet::RegulatoryElement {  // we have to inherit from the abstract regulatoryElement
 public:
  // lanelet2 looks for this string when matching the subtype of a regulatory element to the respective type
  static constexpr char RuleName[] = "lights_on";

  // returns the line where we are supposed to stop
  lanelet::ConstLineString3d fromWhere() const {
    return getParameters<lanelet::ConstLineString3d>(lanelet::RoleName::RefLine).front();
  }

 private:
  LightsOn(lanelet::Id id, lanelet::LineString3d fromWhere)
      : RegulatoryElement{std::make_shared<lanelet::RegulatoryElementData>(id)} {
    parameters().insert({lanelet::RoleNameString::RefLine, {fromWhere}});
  }

  // the following lines are required so that lanelet2 can create this object when loading a map with this regulatory
  // element
  friend class lanelet::RegisterRegulatoryElement<LightsOn>;
  explicit LightsOn(const lanelet::RegulatoryElementDataPtr& data) : RegulatoryElement(data) {}
};

#if __cplusplus < 201703L
constexpr char LightsOn::RuleName[];  // instanciate string in cpp file
#endif
}  // namespace example

namespace {
// this object actually does the registration work for us
lanelet::RegisterRegulatoryElement<example::LightsOn> reg;
}  // namespace

void part3AddingNewRegulatoryElements() {
  using namespace lanelet;
  // after creating our new class and registering it, we can test if it works. For that we use the
  // RegulatoryElementFactory that is used by Lanelet2_io when loading a map. If we did it right, it should now return
  // a regulatory element of the LightsOn class.

  // for that we create a valid regulatory element data object
  LineString3d fromWhere = examples::getLineStringAtX(1);
  RuleParameterMap rules{{RoleNameString::RefLine, {fromWhere}}};

  RegulatoryElementPtr regelem = RegulatoryElementFactory::create("lights_on", utils::getId(), rules);

  // now we can add it to a lanelet and query for it
  Lanelet lanelet = examples::getALanelet();
  lanelet.addRegulatoryElement(regelem);
  assert(!lanelet.regulatoryElementsAs<example::LightsOn>().empty());
}

03_lanelet_map

#include <lanelet2_core/geometry/BoundingBox.h>
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/geometry/Point.h>
#include <lanelet2_core/primitives/Lanelet.h>

#include "lanelet2_examples/internal/ExampleHelpers.h"

#pragma GCC diagnostic ignored "-Wunused-variable"

// we want assert statements to work in release mode
#undef NDEBUG

void part1AboutLaneletMaps();
void part2CreatingLaneletMaps();
void part3QueryingInformation();
void part4LaneletSubmaps();

int main() {
  part1AboutLaneletMaps();
  part2CreatingLaneletMaps();
  part3QueryingInformation();
  part4LaneletSubmaps();
  return 0;
}

void part1AboutLaneletMaps() {
  using namespace lanelet;
  // lanelet maps是所有类型的lanelets primitives的集合体
  LaneletMap map = examples::getALaneletMap(); 

  // Lanelet的地图被由很多层组成，每种primitive是一层
  PointLayer& points = map.pointLayer;
  LineStringLayer& linestrings = map.lineStringLayer;

  // 层里的元素是无序的，可以通过迭代或id进行查找
  assert(points.size() > 1);
  Point3d aPoint = *points.begin();
  Point3d samePoint = *points.find(aPoint.id());
  assert(samePoint == aPoint);
  assert(points.exists(aPoint.id()));

  assert(!linestrings.empty());

  // LaneletMap类型的数据不能复制，但是可以进行move操作
  LaneletMap newMap = std::move(map);
  // map.exists(aPoint.id()); // move操作之后，move之前的地图不再有效
  assert(newMap.pointLayer.exists(aPoint.id()));

  // or be passed around as a pointer:
  LaneletMapUPtr mapPtr = std::make_unique<LaneletMap>(std::move(newMap));
  
  // 常量类型
  // there is also the concept of constness for lanelet maps: const maps return const primitives.
  const LaneletMap& constMap = *mapPtr;
  ConstPoint3d aConstPoint = *constMap.pointLayer.begin();
  assert(aConstPoint == aPoint);
}

void part2CreatingLaneletMaps() {
  using namespace lanelet;
  // 可以通过两种方式创建地图: 逐一向地图添加primitive，或直接从多个primitives创建地图
  LaneletMap laneletMap;

  Lanelet lanelet = examples::getALanelet();
  laneletMap.add(lanelet);
  assert(laneletMap.laneletLayer.size() == 1);

  // 添加Lanelet层元素的时候，points, linestrings, regulatory elements等primitives也会被添加
  assert(!laneletMap.pointLayer.empty());
  assert(laneletMap.pointLayer.exists(lanelet.leftBound().front().id()));

  // InvalId用于指向还不是lanelet map一部分的primitive。这类primitive被添加到lanelet map后，LaneletMap会给他们赋予id
  Point3d invalPoint1(InvalId, 0, 0, 0);
  Point3d invalPoint2(InvalId, 1, 0, 0);
  LineString3d invalLs(InvalId, {invalPoint1, invalPoint2});
  laneletMap.add(invalLs);
  assert(invalPoint1.id() != InvalId);

  // 用createMap()直接从多个primitives创建地图
  Lanelets lanelets{examples::getALanelet(), examples::getALanelet(), examples::getALanelet()};
  LaneletMapUPtr laneletsMap = utils::createMap(lanelets);
  assert(laneletsMap->laneletLayer.exists(lanelets.front().id()));
}

void part3QueryingInformation() {
  using namespace lanelet;
  
  // 除了通过id找LaneletMap中的primitives，还可以通过它们的关系和通过它们的位置找
  LaneletMap laneletMap = examples::getALaneletMap();
  Lanelet mapLanelet = *laneletMap.laneletLayer.begin();
  TrafficLight::Ptr trafficLight = mapLanelet.regulatoryElementsAs<TrafficLight>().front();

  // 可以通过points找Linestrings，通过linestrings或者regulatory element找lanelets和areas，通过任何相关的找regulatory elements

  // 通过linestrings找到lanelets：找到laneletMap中，所有包含mapLanelet.leftBound()的Lanelet
  auto laneletsOwningLinestring = laneletMap.laneletLayer.findUsages(mapLanelet.leftBound());
  assert(laneletsOwningLinestring.size() == 1 && laneletsOwningLinestring.front() == mapLanelet);
  // 通过linestrings找到regelems：找到laneletMap中代表trafficLight的lineString被赋予的所有regulatory elements
  auto regelemsOwningLs =
      laneletMap.regulatoryElementLayer.findUsages(*trafficLight->trafficLights().front().lineString());
  assert(regelemsOwningLs.size() == 1 && regelemsOwningLs.front() == trafficLight);
  // 通过regelems找到lanelets：找到laneletMap中引用了trafficLight的所有lanelet
  auto laneletsOwningRegelems = laneletMap.laneletLayer.findUsages(trafficLight);
  assert(!laneletsOwningRegelems.empty());

  // spacially means we can find primitives with geometrical queries. Because internally all primitives are stored as
  // bounding boxes, these queries only return the primitive with respect to their *bounding box*.
  // 通过几何关系查询primitives
  // 所有的primitives作为bounding boxes储存，通过几何关系查询可以得到bounding boxes产生交互的primitives
  Lanelets lanelets = laneletMap.laneletLayer.nearest(BasicPoint2d(0, 0), 1);  // 1 means the nearest "1" lanelets
  assert(!lanelets.empty());  // 查询得到持有和点(0, 0)的bound ing box最近bounding box的lanelet

  // to get the actually closest lanelets use this utility function:
  // 找到实际最近的lanelet，而不是bounding box最近
  std::vector<std::pair<double, Lanelet>> actuallyNearestLanelets =
      geometry::findNearest(laneletMap.laneletLayer, BasicPoint2d(0, 0), 1);
  assert(!actuallyNearestLanelets.empty());

  // finally we can get primitives using a search region (this also runs on the bounding boxes):
  // 通过指定左上和右下点，划出ROI box，.search找到其中的lanelet
  Lanelets inRegion = laneletMap.laneletLayer.search(BoundingBox2d(BasicPoint2d(0, 0), BasicPoint2d(10, 10)));
  assert(!inRegion.empty());  // IOU>0的所有lanelet

  // for advanced usage, there are the searchUntil and nearestUntil functions. You pass it a function that is called
  // with primitives with increasing bounding box distance until the function returns true. This is then the returned
  // primitive:
  
  // 得到持有和query点的bounding box的举例大于3m的bounding box的第一个lanelet
  // in this example we get the first lanelet whose bounding box distance is >3m distance to the query point
  BasicPoint2d searchPoint = BasicPoint2d(10, 10);
  // the search func is called with the bounding box of a primitive and the primitive itself
  auto searchFunc = [&searchPoint](const BoundingBox2d& lltBox, const Lanelet& /*llt*/) {
    return geometry::distance(searchPoint, lltBox) > 3;
  };
  Optional<Lanelet> lanelet = laneletMap.laneletLayer.nearestUntil(searchPoint, searchFunc);
  assert(!!lanelet && geometry::distance(geometry::boundingBox2d(*lanelet), searchPoint) > 3);
}

void part4LaneletSubmaps() {
  using namespace lanelet;
  // While LaneletMap has the property that when an element is added, all the things referenced by it are added as well,
  // LaneletSubmap does not have this property. This can be useful if you want to avoid that if you add a Lanelet, all
  // Lanelets referenced by its RegulatoryElements are added as well. Apart from that, basically everything you can do
  // with a LaneletMap can also be done with a LaneletSubmap:

  // LaneletSubmap不会添加相关的primitives，例如添加Lanelet层元素的时候，points, linestrings, regulatory elements等primitives不会被添加 
  LaneletSubmap submap{examples::getALaneletMap()};  // it can be constructed (moved) from an existing map

  // you can search its layers
  Lanelets inRegion = submap.laneletLayer.search(BoundingBox2d(BasicPoint2d(0, 0), BasicPoint2d(10, 10)));

  // you can create new submaps from some elements
  LaneletSubmapUPtr newSubmap = utils::createSubmap(inRegion);

  // but this submap will not contain any elements except for the ones you explicitly added
  assert(newSubmap->pointLayer.empty());
  assert(newSubmap->size() == inRegion.size());

  // ... unless you convert back into a laneletMap. This will again contain all primitives:
  LaneletMapUPtr newMap = newSubmap->laneletMap();
  assert(!newMap->pointLayer.empty());
}