GRCC hdmap
手机阅读
这段时间阅读了Lanelet2的源码文档,本章将结合代码和文档来阐述Lanelet2地图框架的具体使用方法,比较底层的源码部分由于篇幅限制暂未涉及。Lanelet2地图框架的基本概念和软件架构在上一篇博客中已经做了讲解。
Lanelet2地图概述 我们先看一下地图数据文件,关于地图格式官方推荐的是OSM格式(XML),开源可编辑。当然也可以是二进制文件,加载速度快,缺点是不可直接编辑。下面的地图文件是按XML形式来存储的,结构十分明朗。关于OSM规范,可以参考其WIKI,https://wiki.openstreetmap.org/wiki/Map_Features
。
结合上一篇我们可以简单地看到,地图里面的每一个3D点都会用一个node标签存储,包含其经纬度LaLon,id是其唯一标志。后续的way标签存储lineString形式的元素,通过id引用node节点存储的3D点。而和交规相关的relation标签则又引用这些way id。
我们在JOSM编辑器中打开可以看到地图的基本形状,此编辑器的使用方法可查看官网https://learnosm.org/en/josm/correcting-imagery-offset/
。下图是一个复杂的十字路口,地图上所有的黄色小框框都是一个3D/2D坐标点。这里红色的线是地图里面的stop_line,在软件界面右侧可以看到其属性是stop line
,并且有四个节点。地图上画出来了所有的车道线,路沿,人行横道,绿化带区域等,还有各个road user
(比如行人,车辆等)需要遵守的交通规则,以及车辆在十字路口可能的换道路线(现实中是没有的,属性定义为virtual)。
Lanelet2代码的安装可以参考官方文档https://github.com/JokerJohn/Lanelet2,其层级结构如下
我们常用的代码在lanelet_core、lanelet_io、lanelet_projection、lanelet_routing和lanelet_traffic_rules这几个模块中。项目采用gtest框架来测试,阅读起来比较容易。
这里我们首先需要知道如何去使用,首先是图元的基本定义方法(增删改查),还有交通规则的定义方法及其与车道和road user
的关联关系。之后是如何去构建、加载使用OSM地图,并且根据需求查询相关的车道等信息。*后是如何生成生成路由图,并利用路由图和实际需求进行合理的路径规划。
基本图元对象的使用 主要有Points、Area、Lanelet、LineString、Polygon,首先引入头文件,这些图元的基本处理方法都在lanelet_core模块中,下面将详细介绍其使定义和利用他们来进行几何运算。
1 2 3 4 5 6 7 8 9 10 11 copy #include <lanelet2_core/LaneletMap.h> #include <lanelet2_core/geometry/Area.h> #include <lanelet2_core/geometry/Lanelet.h> #include <lanelet2_core/primitives/Area.h> #include <lanelet2_core/primitives/Lanelet.h> #include <lanelet2_core/primitives/LineString.h> #include <lanelet2_core/primitives/Point.h> #include <lanelet2_core/primitives/Polygon.h> #include <lanelet2_core/utility/Units.h> #include "../ExampleHelpers.h"
lanelet_core模块中使用嵌套的namespace来组织相关的方法,这里我们先引入两个重要的命名空间,后续不一一说明。
1 2 copy using namespace lanelet; using namespace lanelet::units::literals;
比如这里看一下lanelet::units::literals里面定义了距离,速度、加速度、时间相关单位。
Points使用 这里我们开始定义一个Point,通常定义为Point3d,在我们不关注高度的时候可定义为Point2d,在Lanelet2里面是很少会进行变量拷贝的,一般都是指针引用,而地图数据在使用过程中一般不会发生改变,这里我们可以定义为ConstPoint3d等,在代码里面ConstPoint3d和Point3d使用方法完全相同,除了无法修改数据。
每个Point必须要有一个唯一的id,一般为正数,Lanelet2里面提供了相应的id生成器utils::getId()
。这里pCopy完全等于p,当p的数据修改时,pCopy也会共享。
1 2 3 4 5 6 7 8 9 10 copy Point3d p (utils::getId(), 0 , 0 , 0 ) ;Point3d pCopy = p; assert(p == pCopy); p.z() = 2 ; p.setId(utils::getId()); assert(p.id() == pCopy.id()); assert(p.z() == pCopy.z());
下面设定点p的一些属性
1 2 3 4 5 6 7 8 9 10 11 12 13 copy p.attributes()["type" ] = "point" ; p.attributes()["pi" ] = 3.14 ; p.attributes()["velocity" ] = "5 kmh" ; assert(p.attributes() == pCopy.attributes()); assert(p.attribute("type" ) == "point" ); assert(!!p.attribute("pi" ).asDouble()); assert(p.attribute("velocity" ).asVelocity() == 5 _kmh); assert(p.attributeOr("nonexistent" , -1 ) == -1 ); assert(p.attributeOr("type" , 0 ) == 0 ); assert(p.attributeOr("velocity" , 0 _kmh) == 5 _kmh);
Point3d变量可以转化成ConstPoint3d,但反过来不行。
1 2 3 4 5 6 7 8 9 10 11 12 copy ConstPoint3d pConst = p; assert(pConst.z() == 2 ); p.z() = 3 ; assert(pConst.z() == 3 ); ConstPoint3d &pConstRef = p; assert(pConstRef.z() == 3 ); assert(p.constData() == pConst.constData());
Point3d与Point2d相互转换
1 2 3 4 5 6 copy Point3d p3d (utils::getId(), 1 , 2 , 3 ) ;BasicPoint3d &p3dBasic = p3d.basicPoint(); Point2d p2d = utils::to2D(p3d); Point3d p3dNew = utils::to3D(p2d); assert(p3dNew.constData() == p3d.constData());
LineString使用 LineString至少要求有一个点,下述代码创建一条连接点(0,0,0)、(1,0,0)、(2,0,0)的LineString。
1 2 3 copy Point3d p1{utils::getId(), 0 , 0 , 0 }, p2{utils::getId(), 1 , 0 , 0 }, p3{utils::getId(), 2 , 0 , 0 }; LineString3d ls (utils::getId(), {p1, p2, p3}) ;
LineString在使用上和vector类似,可以进行push和pop
1 2 3 4 5 6 7 8 9 copy assert(ls[1 ] == p2); assert(ls.size() == 3 ); for (Point3d &p : ls) { assert(p.y() == 0 ); } ls.push_back(Point3d(utils::getId(), 3 , 0 , 0 )); assert(ls.size() == 4 ); ls.pop_back(); assert(ls.size() == 3 );
LineString也可以设置一些属性,比如细线,虚线等
1 2 copy ls.attributes()[AttributeName::Type] = AttributeValueString::LineThin; ls.attributes()[AttributeName::Subtype] = AttributeValueString::Dashed;
在LineString中,两个相邻点组成线段,除了遍历点,还可以遍历线段。
1 2 3 4 copy assert(ls.numSegments() >= 2 ); Segment3d segment = ls.segment(1 ); assert(segment.first == p2); assert(segment.second == p3);
LineString也可以进行反转,反转两次后和原来的LineString一样。
1 2 3 4 5 6 7 8 copy LineString3d lsInv = ls.invert(); assert(lsInv.front() == p3); assert(lsInv.inverted()); assert(lsInv.constData() == ls.constData()); LineString3d lsInvInv = lsInv.invert(); assert(lsInvInv == ls); assert(lsInvInv.front() == p1);
LineString也有const,2d版本。
1 2 3 4 5 6 7 8 copy ConstLineString3d lsConst = ls; ConstPoint3d p1Const = ls[0 ]; assert(lsConst.constData() == ls.constData()); LineString2d ls2d = utils::to2D(ls); Point2d front2d = ls2d.front(); assert(front2d == utils::to2D(p1));
LinString还有一个混合版本ConstHybridLineString3d,BasicLineString3d。 这有点特殊,因为它返回BasicPoints(也就是Eigen点),非常适合几何计算,特别是后面使用boost.geometry库时。
1 2 3 4 5 6 copy ConstHybridLineString3d lsHybrid = utils::toHybrid(ls); BasicPoint3d p1Basic = lsHybrid[0 ]; assert(p1Basic.x() == p1.x()); BasicLineString3d lsBasic = ls.basicLineString(); assert(lsBasic.front() == lsHybrid.front());
Polygon的使用 和LineString使用方法类似,重要的区别就是首末点是连接成线段的,是闭合的,而且Polygon是无法反转的,默认方向是顺时针。Polygon的类型和LineString一致,const, 2d, 3d, hybrid。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 copy Point3d p1{utils::getId(), 0 , 0 , 0 }, p2{utils::getId(), 1 , 0 , 0 }, p3{utils::getId(), 2 , -1 , 0 }; Polygon3d poly (utils::getId(), {p1, p2, p3}) ;assert(poly.size() == 3 ); assert(poly.numSegments() == 3 ); Segment3d lastSegment = poly.segment(2 ); assert(lastSegment.first == p3); assert(lastSegment.second == p1); ConstPolygon3d polyConst = poly; ConstHybridPolygon3d polyHybrid = utils::toHybrid(poly); ConstPolygon2d poly2dConst = utils::to2D(polyConst); assert(polyHybrid.constData() == poly2dConst.constData());
Lanelet使用 Lanelet有左右两条边界,也就是两条LineString
1 2 3 4 5 6 7 copy LineString3d left = examples::getLineStringAtY(1 ); LineString3d right = examples::getLineStringAtY(0 ); Lanelet lanelet (utils::getId(), left, right) ;lanelet.setLeftBound(left); assert(lanelet.leftBound() == left); assert(lanelet.rightBound() == right);
这里getLineStringAtY
就是生成两条示例线段
Lanelet还有centerLine的概念,但是自动计算的,且不可修改。并且中心线在自动计算之后会被缓存起来,当Lanelet的左右边界发生变化时,这个缓存会被清空。但是左右边界修改时,系统不会自动提醒,需要reset缓存。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 copy ConstLineString3d centerline = lanelet.centerline(); ConstLineString3d centerline2 = lanelet.centerline(); assert(centerline2 == centerline); lanelet.setLeftBound(examples::getLineStringAtY(2 )); ConstLineString3d centerline3 = lanelet.centerline(); assert(centerline3 != centerline); right.push_back(Point3d(utils::getId(), 4 , 0 , 0 )); assert(centerline3 == lanelet.centerline()); lanelet.resetCache(); assert(centerline3 != lanelet.centerline()); right.pop_back(); lanelet.resetCache();
Lanelet还可以进行反转,实际上就是左右边界分别反转,现实中就是表示双向车道。
1 2 3 copy Lanelet laneletInv = lanelet.invert(); assert(laneletInv.leftBound().front() == lanelet.rightBound().back()); assert(laneletInv.constData() == lanelet.constData());
进行几何计算的时候会经常用到Lanelet的轮廓,这里我们用CompoundPolygon类型来表示,看起来像之前提到的Polygon类型,实际上是由多个线串一起按顺时针方向组合形成的。Lanelet由const版本,但是没有3d或者2d版本
1 2 3 4 5 6 7 8 9 copy CompoundPolygon3d polygon = lanelet.polygon3d(); assert(polygon.size() == 6 ); assert(polygon[0 ] == lanelet.leftBound().front()); assert(polygon.back() == lanelet.rightBound().front()); ConstLanelet laneletConst = lanelet; assert(laneletConst.constData() == lanelet.constData()); ConstLineString3d leftConst = lanelet.leftBound();
Lanelet有相关联的regElem的,后续再讲解。
1 copy assert(lanelet.regulatoryElements().empty());
Area的使用 Area的使用和Lanelet差不多,区别就是Area没有左右边界的概念,而是由LineString组成的集合,按顺时针方向连接在一起形成外围边界。Area可以用来描述坑这种有内外框的情况,内框是逆时针方向。他们都有const版本和相关联的regElem。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 copy LineString3d top = examples::getLineStringAtY(2 ); LineString3d right = examples::getLineStringAtX(2 ).invert(); LineString3d bottom = examples::getLineStringAtY(0 ).invert(); LineString3d left = examples::getLineStringAtY(0 ); Area area (utils::getId(), {top, right, bottom, left}) ;LineStrings3d outer = area.outerBound(); area.setOuterBound(outer); CompoundPolygon3d outerPolygon = area.outerBoundPolygon(); assert(area.innerBounds().empty()); ConstArea areaConst = area; ConstLineStrings3d outerConst = areaConst.outerBound(); assert(area.regulatoryElements().empty());
几何计算 这里我们运用定义好的基本类型进行几何计算,比如计算距离等。这里我们定义一个Point(1,4,0),一条LineString(从(0,2,0)到(2,2,0)),一个Polygon,一个Lanelet(x:0->3,y:2->0),一个Area。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 copy Point3d point (utils::getId(), 1 , 4 , 1 ) ;LineString3d ls = examples::getLineStringAtY(2 ); Polygon3d poly = examples::getAPolygon(); Lanelet lanelet = examples::getALanelet(); Area area = examples::getAnArea(); ConstHybridLineString3d lsHybrid = utils::toHybrid(ls); ConstHybridPolygon3d polyHybrid = utils::toHybrid(poly); auto dP2Line3d = geometry::distance(point, lsHybrid);assert(dP2Line3d > 2 ); auto dP2Line2d = geometry::distance(utils::to2D(point), utils::to2D(lsHybrid));assert(dP2Line2d == 2 ); auto l3d = geometry::length(lsHybrid);assert(l3d == 2 ); auto ar = geometry::area(utils::to2D(polyHybrid)); assert(ar == 2 ); BasicPoint3d pProj = geometry::project(ls, point); assert(pProj.y() == 2 ); ArcCoordinates arcCoordinates = geometry::toArcCoordinates(utils::to2D(ls),utils::to2D(point)); assert(arcCoordinates.distance == 2 ); assert(arcCoordinates.length == 1 );
我们现在可以使用bounding boxes进行有效的交点估计。 如果两个框仅接触但共享区域仍为0,则相交也返回true。如果想求的不仅仅是估计,则可以直接使用基本图元。 仅当两个图元的共享区域大于0时,Overlaps才返回true。 在3d中,z中的距离也必须小于边距。
1 2 3 4 5 6 7 8 9 copy BoundingBox3d pointBox = geometry::boundingBox3d(point); BoundingBox3d lsBox = geometry::boundingBox3d(ls); BoundingBox2d laneletBox = geometry::boundingBox2d(lanelet); BoundingBox2d areaBox = geometry::boundingBox2d(area); assert(geometry::intersects(laneletBox, areaBox)); assert(!geometry::intersects(pointBox, lsBox)); assert(geometry::overlaps3d(area, lanelet, 3 ));
RegElem的使用 Lanelet2定义了很多基本的RegElem,例如traffic light 和speed limit,以下主要讲解这些RegElem是如何和lanelet进行关联的,后面的Area,Polygon等关联方法也是一致的。
traffic lights使用LineString来表示的,与其相关联的还有stop line。
1 2 3 4 5 6 copy LineString3d trafficLight = examples::getLineStringAtY(1 ); trafficLight.attributes()[AttributeName::Type] = AttributeValueString::TrafficLight; RegulatoryElementPtr trafficLightRegelem = lanelet::TrafficLight::make(utils::getId(), {}, {trafficLight});
创建好了相应的RegElem需要与相应的lanelet关联起来。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 copy Lanelet lanelet = examples::getALanelet(); lanelet.addRegulatoryElement(trafficLightRegelem); assert(lanelet.regulatoryElements().size() == 1 ); RegulatoryElementPtr regelem = lanelet.regulatoryElements()[0 ]; assert(lanelet.regulatoryElementsAs<SpeedLimit>().empty()); std ::vector <TrafficLight::Ptr> trafficLightRegelems = lanelet.regulatoryElementsAs<TrafficLight>();assert(trafficLightRegelems.size() == 1 ); TrafficLight::Ptr tlRegelem = trafficLightRegelems.front(); assert(tlRegelem->constData() == trafficLightRegelem->constData());
traffic lights也可以用Polygon来描述,这里可以有一种兼容写法。需要注意的是,我们可以修改RegElem,但一个RegElem可能与多个lanelet关联起来了,是共享数据,如果修改的话会影响其他的lanelets。
1 2 3 4 5 copy LineStringOrPolygon3d theLight = tlRegelem->trafficLights().front(); assert(theLight == trafficLight); tlRegelem->setStopLine(examples::getLineStringAtY(2 )); assert(!!tlRegelem->stopLine());
为了编码方便,我们可以采用GenericRegulatoryElement对任何交通规则进行建模。但是,实际工程中一般不使用,因为它的通用结构很难理解。以下代码,我们给某一个road user增加其相关联的图元。
1 2 3 4 5 6 7 8 9 10 copy GenericRegulatoryElement regelem (utils::getId()) ;Lanelet lanelet = examples::getALanelet(); regelem.addParameter(RoleName::Refers, lanelet); Point3d point (utils::getId(), 0 , 0 , 0 ) ;regelem.addParameter(RoleName::Refers, point); Points3d pts = regelem.getParameters<Point3d>(RoleName::Refers); assert(!pts.empty() && pts.front() == point);
现在我们来注册一个新的RegElem元素到Lanelet2中,叫做LightsOn,它代表车辆能够在经过特定的line的时候打开车灯。首先LightsOn类应该继承自RegulatoryElement类。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 copy namespace example { class LightsOn : public lanelet::RegulatoryElement { public : static constexpr char RuleName[] = "lights_on" ; 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}}); } friend class lanelet : :RegisterRegulatoryElement<LightsOn>; explicit LightsOn(const lanelet::RegulatoryElementDataPtr &data) : RegulatoryElement(data) {} }; #if __cplusplus < 201703L constexpr char LightsOn::RuleName[]; #endif } namespace { lanelet::RegisterRegulatoryElement<example::LightsOn> reg; }
注册好了rule类型之后,下面是具体的关联方法。RegulatoryElementFactory工厂类是在lanelet_io中定义,在加载地图的时候自动创建,去注册相关的RegElem和相应的rule,*后与特定的lanelet关联起来。
1 2 3 4 5 6 7 8 copy LineString3d fromWhere = examples::getLineStringAtX(1 ); RuleParameterMap rules{{RoleNameString::RefLine, {fromWhere}}}; RegulatoryElementPtr regelem = RegulatoryElementFactory::create("lights_on" , utils::getId(), rules); Lanelet lanelet = examples::getALanelet(); lanelet.addRegulatoryElement(regelem); assert(!lanelet.regulatoryElementsAs<example::LightsOn>().empty());
LaneletMap及IO LaneletMap使用 LaneletMap类可以理解为管理所有创建好的图元数据的容器,它提供了查询各种地图数据的基本方法。按之前的分层,LaneletMap也分为pointLayer和lineStringLayer两层,其他图元都可以用此两层的通用方法来处理。每一层都类似于无序地图,需要经过遍历或者通过id查询。注意下面的map是无法被拷贝的,而是通过std::move将左值引用转换为右值引用,进行变量的“转移”,这也符合我们之前分析的,尽量避免数据的拷贝操作。在进行move之后,原来的map变量是清空的。*后一点是和先前一样,LaneletMap也有const版本。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 copy LaneletMap map = examples::getALaneletMap(); PointLayer &points = map .pointLayer; LineStringLayer &linestrings = map .lineStringLayer; Point3d aPoint = *points.begin(); Point3d samePoint = *points.find(aPoint.id()); assert(samePoint == aPoint); assert(points.exists(aPoint.id())); assert(!linestrings.empty()); LaneletMap newMap = std ::move(map ); assert(newMap.pointLayer.exists(aPoint.id())); LaneletMapUPtr mapPtr = std ::make_unique<LaneletMap>(std ::move(newMap)); const LaneletMap &constMap = *mapPtr;ConstPoint3d aConstPoint = *constMap.pointLayer.begin(); assert(aConstPoint == aPoint);
LaneletMap生成 下面演示map的创建方法,无非定义对象,给实例填充数据。一般有两种方法,一是少量数据一个个的填充,二是直接从多个对象中创建。在添加对象的时候,会自动加入其相关属性,(points, linestrings, regulatory elements)。
1 2 3 4 5 6 7 8 9 10 11 12 copy LaneletMap laneletMap; Lanelet lanelet = examples::getALanelet(); laneletMap.add(lanelet); assert(laneletMap.laneletLayer.size() == 1 ); assert(!laneletMap.pointLayer.empty()); assert(laneletMap.pointLayer.exists(lanelet.leftBound().front().id())); Lanelets lanelets{examples::getALanelet(), examples::getALanelet(), examples::getALanelet()}; LaneletMapUPtr laneletsMap = utils::createMap(lanelets); assert(laneletsMap->laneletLayer.exists(lanelets.front().id()));
之前我们定义的所有id均为正整数,那么也就可以用id=0来标识不属于此地图中的数据。在添加无效id的图元数据时,Lanlet2会自动重新给它生成有效id。
1 2 3 4 5 copy Point3d invalPoint1 (InvalId, 0 , 0 , 0 ) ;Point3d invalPoint2 (InvalId, 1 , 0 , 0 ) ;LineString3d invalLs (InvalId, {invalPoint1, invalPoint2}) ;laneletMap.add(invalLs); assert(invalPoint1.id() != InvalId);
地图数据查询 LaneletMap提供两种数据查询方法,一是根据其关联关系,比如可根据当前lanelet查询其关联的traffic light(regelems)和stop line(linestring),根据Points查询Linestring,二是根据位置,比如查询当前lanelet 50米范围内所有的lanelet,RegElem等。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 copy LaneletMap laneletMap = examples::getALaneletMap(); Lanelet mapLanelet = *laneletMap.laneletLayer.begin(); TrafficLight::Ptr trafficLight = mapLanelet.regulatoryElementsAs<TrafficLight>().front(); auto laneletsOwningLinestring = laneletMap.laneletLayer.findUsages(mapLanelet.leftBound());assert(laneletsOwningLinestring.size() == 1 && laneletsOwningLinestring.front() == mapLanelet); auto regelemsOwningLs = laneletMap.regulatoryElementLayer.findUsages(*trafficLight->trafficLights().front().lineString());assert(regelemsOwningLs.size() == 1 && regelemsOwningLs.front() == trafficLight); auto laneletsOwningRegelems = laneletMap.laneletLayer.findUsages(trafficLight);assert(!laneletsOwningRegelems.empty());
下面是根据位置查询,从空间上意味着我们可以使用几何查询找到图元。 由于内部所有的图元都存储为边界框,因此这些查询仅返回关于它们的“边界框”的图元。对于高级用法,有searchUntil和nearestUntil函数。 向它传递一个函数,该函数将使用具有增加的边界框距离的图元函数调用,直到该函数返回true。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 copy Lanelets lanelets = laneletMap.laneletLayer.nearest(BasicPoint2d(0 , 0 ), 1 ); assert(!lanelets.empty()); std ::vector <std ::pair<double , Lanelet>> actuallyNearestLanelets = geometry::findNearest(laneletMap.laneletLayer, BasicPoint2d(0 , 0 ), 1 ); assert(!actuallyNearestLanelets.empty()); Lanelets inRegion = laneletMap.laneletLayer.search(BoundingBox2d(BasicPoint2d(0 , 0 ), BasicPoint2d(10 , 10 ))); assert(!inRegion.empty()); BasicPoint2d searchPoint = BasicPoint2d(10 , 10 ); auto searchFunc = [&searchPoint](const BoundingBox2d &lltBox, const Lanelet & ) { return geometry::distance(searchPoint, lltBox) > 3 ; }; Optional <Lanelet> lanelet = laneletMap.laneletLayer.nearestUntil(searchPoint, searchFunc); assert(!!lanelet && geometry::distance(geometry::boundingBox2d(*lanelet), searchPoint) > 3 );
上面的getALaneletMap
表示生成示例地图,一个Area,一条Lanelet和一个相关联的RegElem。
LaneletSubmap子图 LaneletMap类包含了所有处理地图数据的方法,此外还有LaneletSubmap不包含RegElem,除非在创建LaneletSubmap时明确加入的关联。
1 2 3 4 5 6 7 8 9 10 11 12 13 copy LaneletSubmap submap{examples::getALaneletMap()}; Lanelets inRegion = submap.laneletLayer.search(BoundingBox2d(BasicPoint2d(0 , 0 ), BasicPoint2d(10 , 10 ))); LaneletSubmapUPtr newSubmap = utils::createSubmap(inRegion); assert(newSubmap->pointLayer.empty()); assert(newSubmap->size() == inRegion.size()); LaneletMapUPtr newMap = newSubmap->laneletMap(); assert(!newMap->pointLayer.empty());
现实OSM地图的加载与生成 继续文章开头内容,我们来读取示例的OSM地图数据。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 copy #include <lanelet2_core/primitives/Lanelet.h> #include <lanelet2_io/Io.h> #include <lanelet2_io/io_handlers/Factory.h> #include <lanelet2_io/io_handlers/Writer.h> #include <lanelet2_projection/UTM.h> #include <cstdio> namespace { std ::string exampleMapPath = std ::string (PKG_DIR) + "/../lanelet2_maps/res/mapping_example.osm" ; std ::string tempfile (const std ::string &name) { char tmpDir[] = "/tmp/lanelet2_example_XXXXXX" ; auto file = mkdtemp(tmpDir); if (file == nullptr ) { throw lanelet::IOError("Failed to open a temporary file for writing" ); } return std ::string (file) + '/' + name; } }
注意,加载真实地图的时候需要考虑坐标系转换,一般是地理系与平面系的转换,Lanelet中推荐转换的地理系是UTM系。考虑地理系,就必须知道地图起点的经纬LaLon坐标,在Lanelet2中定义了projector来专门处理lat/lon->x/y。
1 2 3 4 5 6 7 8 9 10 copy projection::UtmProjector projector (Origin({49 , 8.4 })) ; LaneletMapPtr map = load(exampleMapPath, projector); write(tempfile("map.bin" ), *map ); ErrorMessages errors; map = load(exampleMapPath, projector, &errors);assert(errors.empty());
Lanelet2中也提供了经纬转UTM坐标的处理方法。
1 2 3 4 5 6 7 copy projection::UtmProjector projector (Origin({49 , 8.4 })) ; BasicPoint3d projection = projector.forward(GPSPoint{49 , 8.4 , 0 }); assert(std ::abs (projection.x()) < 1e-6 );
此外,不只是OSM格式,我们也可以自定义解析器来解析符合Lanet2要求的其他格式地图数据。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 copy namespace example { class FakeWriter : public lanelet::io_handlers::Writer { public : using Writer::Writer; void write (const std ::string & , const lanelet::LaneletMap & ,lanelet::ErrorMessages & ) const override { } static constexpr const char *extension () { return ".fake" ; } static constexpr const char *name () { return "fake_writer" ; } }; } namespace { lanelet::io_handlers::RegisterWriter <example::FakeWriter> reg; }
以上我们注册了解析.fake格式数据的解析器,使用起来就很方便。
1 2 copy LaneletMap map ; write("anypath.fake" , map );
使用交规对象解释地图数据 本节主要讲解如何使用Traffic rules来解释地图数据。这里的解释主要是将我们已经建模好的关联关系,让汽车或者其他road user容易理解。这里我们构建三条横向连续的lanelet,分别为left、middle、right,其边界分别为left/middleLs、middleLs/rightLs、nextLeftLs/nextRightLs。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 copy #include <lanelet2_core/utility/Units.h> #include <lanelet2_traffic_rules/TrafficRules.h> #include <lanelet2_traffic_rules/TrafficRulesFactory.h> using namespace lanelet::units::literals;LineString3d leftLs{examples::getLineStringAtY(3 )}, middleLs{examples::getLineStringAtY(2 )}, rightLs{examples::getLineStringAtY(0 )}; LineString3d nextLeftLs{utils::getId(), {middleLs.back(), Point3d(utils::getId(), middleLs.back().x() + 1. , middleLs.back().y())}}; LineString3d nextRightLs{utils::getId(),{rightLs.back(), Point3d(utils::getId(), rightLs.back().x() + 1 , rightLs.back().y())}}; Lanelet left{utils::getId(), leftLs, middleLs}; Lanelet right{utils::getId(), middleLs, rightLs}; Lanelet next{utils::getId(), nextLeftLs, nextRightLs};
为了知道当前位置,哪些地方是可以通行的,我们需要知道相关联的具体的交规。我们使用TrafficRulesFactory来获取当前lanelet相关联的所有road user的所有rule。之后我们就能确定此处车辆和行人是否可通行,当前车辆能否变道。默认的lanelet只有一个方向,如何当前行人和车辆均不可通行,那么反转之后的lanelet也不可通行。我们也能查询当前的限速信息。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 copy traffic_rules::TrafficRulesPtr trafficRules = traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle); traffic_rules::TrafficRulesPtr pedestrianRules = traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Pedestrian); assert(trafficRules->canPass(right)); assert(!pedestrianRules->canPass(right)); assert(!trafficRules->canPass(right.invert())); assert(!trafficRules->canPass(right, left)); traffic_rules::SpeedLimitInformation limit = trafficRules->speedLimit(right); assert(limit.speedLimit == 50 _kmh); assert(limit.isMandatory); middleLs.attributes()[AttributeName::Type] = AttributeValueString::LineThin; middleLs.attributes()[AttributeName::Subtype] = AttributeValueString::Dashed; right.attributes()[AttributeName::Type] = AttributeValueString::Lanelet; right.attributes()[AttributeName::Subtype] = AttributeValueString::Road; right.attributes()[AttributeName::Location] = AttributeValueString::Nonurban; left.attributes() = right.attributes(); next.attributes() = right.attributes(); left.attributes()[AttributeName::OneWay] = false ; assert(trafficRules->canChangeLane(right, left)); assert(trafficRules->canChangeLane(left, right)); assert(trafficRules->canPass(left.invert())); assert(!trafficRules->canChangeLane(left, right.invert())); assert(!trafficRules->canChangeLane(left.invert(), right.invert())); limit = trafficRules->speedLimit(right); assert(limit.speedLimit == 100 _kmh); LineString3d sign = examples::getLineStringAtX(3 ); SpeedLimit::Ptr speedLimit = SpeedLimit::make(utils::getId(), {}, {{sign}, "de274-60" }); right.addRegulatoryElement(speedLimit); assert(trafficRules->speedLimit(right).speedLimit == 60 _kmh); right.attributes()[AttributeName::Subtype] = AttributeValueString::Crosswalk; assert(!trafficRules->canPass(right)); assert(pedestrianRules->canPass(right));
路由图的生成及使用 本节主要讲解如何根据加载的地图,创建和使用路由图。从map,交规,routing cost中可以得到路由图。路由代价主要是根据距离和时间(限速信息)综合计算,路由图可用于查询三种不同的信息:
特定Lanelet/Area的相邻关系
来自Lanelet/Area的可能通行的路线
两个Lanelet之间的*短路线。
绘制路由图 road user的选择会影响从哪个角度生成路由图,下面我们将为车辆绘制路线图。我们还可以搜索冲突的Lanelet/Area,但这只会返回与相同road user的其他Lanelet的冲突。 例如。 此图用于车辆时,这将永远不会返回人行横道。我们还可以从当前出发找出可能的路径。 我们选择的路由代价的id为0,这将为我们提供至少100m长的路由。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 copy LaneletMapPtr map = load(exampleMapPath, projection::UtmProjector(Origin({49 , 8.4 }))); traffic_rules::TrafficRulesPtr trafficRules = traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle); routing::RoutingGraphUPtr routingGraph = routing::RoutingGraph::build(*map , *trafficRules); ConstLanelet lanelet = map ->laneletLayer.get(4984315 ); assert(!routingGraph->adjacentLeft(lanelet)); assert(!routingGraph->adjacentRight(lanelet)); assert(!!routingGraph->right(lanelet)); assert(routingGraph->besides(lanelet).size() == 3 ); assert(routingGraph->following(lanelet).size() == 1 ); assert(routingGraph->conflicting(lanelet).empty()); routing::LaneletPaths paths = routingGraph->possiblePaths(lanelet, 100 , 0 , false ); assert(paths.size() == 1 ); paths = routingGraph->possiblePaths(lanelet, 100 , 0 , true ); assert(paths.size() == 4 ); ConstLanelets reachableSet = routingGraph->reachableSet(lanelet, 100 , 0 ); assert(reachableSet.size() > 10 );
*后是路由内容。 在这里,我们仅关注*短路径。*短路径可以包含(突然的)变道。 对于车辆,应该建议优先选择哪个车道(局部)而不是整条路线(全局)。 可以在路径中查询可以遵循的车道顺序,直到必须换道为止。路由图还具有一种自检机制,该机制可验证图的所有部分是否处于健全状态。
1 2 3 4 5 6 7 8 9 copy ConstLanelet toLanelet = map ->laneletLayer.get(2925017 ); Optional <routing::LaneletPath> shortestPath = routingGraph->shortestPath(lanelet, toLanelet, 1 ); assert(!!shortestPath); LaneletSequence lane = shortestPath->getRemainingLane(shortestPath->begin()); assert(!lane.empty()); routingGraph->checkValidity();
使用路由图 Route对象建立在起点和终点之间的*短路径上。 但是,它不仅包含沿*短路径的lanelets,还包含可用于目的地而无需离开所选道路的所有lanelets。 当需要知道到达目的地的所有选项(包括变道)时,这就是我们要追求的目标。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 copy LaneletMapPtr map = load(exampleMapPath, projection::UtmProjector(Origin({49 , 8.4 }))); traffic_rules::TrafficRulesPtr trafficRules = traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle); routing::RoutingGraphUPtr routingGraph = routing::RoutingGraph::build(*map , *trafficRules); ConstLanelet lanelet = map ->laneletLayer.get(4984315 ); ConstLanelet toLanelet = map ->laneletLayer.get(2925017 ); Optional <routing::Route> route = routingGraph->getRoute(lanelet, toLanelet, 0 ); assert(!!route); routing::LaneletPath shortestPath = route->shortestPath(); assert(!shortestPath.empty()); LaneletSequence fullLane = route->fullLane(lanelet); assert(!fullLane.empty()); auto right = route->rightRelation(lanelet);assert(!!right); LaneletSubmapConstPtr routeMap = route->laneletSubmap(); assert(!routeMap->laneletLayer.empty());
由于路由图仅包含可用于一个特定参与者的原语,因此我们不能使用它们来查询不同road user
之间的可能冲突。 RoutingGraphContainer
可解决此问题:它通过比较不同的拓扑图来查找各个road user
之间的冲突。
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 copy LaneletMapPtr map = load(exampleMapPath, projection::UtmProjector(Origin({49 , 8.4 }))); traffic_rules::TrafficRulesPtr trafficRules = traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle); traffic_rules::TrafficRulesPtr pedestrianRules = traffic_rules::TrafficRulesFactory::create(Locations::Germany, Participants::Pedestrian); routing::RoutingGraphConstPtr vehicleGraph = routing::RoutingGraph::build(*map , *trafficRules); routing::RoutingGraphConstPtr pedestrGraph = routing::RoutingGraph::build(*map , *pedestrianRules); routing::RoutingGraphContainer graphs ({vehicleGraph, pedestrGraph}) ; ConstLanelet lanelet = map ->laneletLayer.get(4984315 ); ConstLanelet intersectLanelet = map ->laneletLayer.get(185265 ); assert(graphs.conflictingInGraph(lanelet, 1 ).empty()); assert(graphs.conflictingInGraph(intersectLanelet, 1 ).size() == 1 );
文章作者: 胡想成
发布时间: 2020年02月25日 - 17:02
*后更新: 2020年03月14日 - 11:03