Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MapMatcher.h
15 : /// @author Jakob Erdmann
16 : /// @date Thu, 7 March 2024
17 : ///
18 : // Utility functions for matching locations to edges (during route parsing)
19 : /****************************************************************************/
20 : #pragma once
21 : #include <config.h>
22 :
23 : #include <utils/geom/GeoConvHelper.h>
24 : #include <utils/vehicle/SUMOVTypeParameter.h>
25 : #include "NamedRTree.h"
26 : #include "RandHelper.h"
27 : #include "SUMOVehicleClass.h"
28 : #include "MsgHandler.h"
29 :
30 : #define JUNCTION_TAZ_MISSING_HELP "\nSet option '--junction-taz' or load a TAZ-file"
31 :
32 : // ===========================================================================
33 : // class declarations
34 : // ===========================================================================
35 :
36 : // ===========================================================================
37 : // class definitions
38 : // ===========================================================================
39 : /**
40 : * @class MapMatcher
41 : * @brief Provides utility functions for matching locations to edges (during route parsing)
42 : */
43 :
44 : template<class E, class L, class N>
45 : class MapMatcher {
46 :
47 : public:
48 839 : void parseGeoEdges(const PositionVector& positions, bool geo, SUMOVehicleClass vClass,
49 : std::vector<const E*>& into, const std::string& rid, bool isFrom, bool& ok, bool forceEdge = false) {
50 839 : if (geo && !GeoConvHelper::getFinal().usingGeoProjection()) {
51 0 : WRITE_ERROR(TL("Cannot convert geo-positions because the network has no geo-reference"));
52 0 : ok = false;
53 0 : return;
54 : }
55 1700 : for (Position pos : positions) {
56 861 : Position orig = pos;
57 861 : if (geo) {
58 164 : GeoConvHelper::getFinal().x2cartesian_const(pos);
59 : }
60 861 : double dist = MIN2(10.0, myMapMatchingDistance);
61 861 : const L* best = getClosestLane(pos, vClass, dist);
62 861 : while (best == nullptr && dist < myMapMatchingDistance) {
63 0 : dist = MIN2(dist * 2, myMapMatchingDistance);
64 0 : best = getClosestLane(pos, vClass, dist);
65 : }
66 861 : if (best == nullptr) {
67 36 : myErrorOutput->inform("No edge found near position " + toString(orig, geo ? gPrecisionGeo : gPrecision) + " within the route " + rid + ".");
68 18 : ok = false;
69 : } else {
70 843 : const E* bestEdge = &best->getEdge();
71 862 : while (bestEdge->isInternal()) {
72 19 : bestEdge = bestEdge->getSuccessors().front();
73 : }
74 843 : if ((myMapMatchJunctions || myMapMatchTAZ) && !forceEdge) {
75 442 : if (myMapMatchTAZ) {
76 69 : bestEdge = getTaz(pos, bestEdge, isFrom);
77 : } else {
78 373 : bestEdge = getJunctionTaz(pos, bestEdge, vClass, isFrom);
79 : }
80 442 : if (bestEdge != nullptr) {
81 427 : into.push_back(bestEdge);
82 : } else {
83 15 : ok = false;
84 : }
85 : } else {
86 : // handle multiple via locations on the same edge without loops
87 401 : if (positions.size() == 1 || into.empty() || into.back() != bestEdge) {
88 379 : into.push_back(bestEdge);
89 : }
90 : }
91 : }
92 : }
93 : }
94 :
95 :
96 :
97 : protected:
98 95986 : MapMatcher(bool matchJunctions, bool matchTAZ, double matchDistance, MsgHandler* errorOutput):
99 95986 : myMapMatchTAZ(matchTAZ),
100 95986 : myLaneTree(nullptr),
101 95986 : myMapMatchJunctions(matchJunctions),
102 95986 : myMapMatchingDistance(matchDistance),
103 95986 : myErrorOutput(errorOutput) {}
104 :
105 : virtual ~MapMatcher() {
106 181 : delete myLaneTree;
107 93222 : }
108 :
109 : /// @brief find closest lane within distance for the given position or nullptr
110 869 : const L* getClosestLane(const Position& pos, SUMOVehicleClass vClass, double distance = -1.) {
111 869 : NamedRTree* t = getLaneTree();
112 869 : Boundary b;
113 869 : b.add(pos);
114 869 : b.grow(distance < 0 ? myMapMatchingDistance : distance);
115 869 : const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
116 869 : const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
117 : std::set<const Named*> lanes;
118 : Named::StoringVisitor sv(lanes);
119 869 : t->Search(cmin, cmax, sv);
120 : // use closest
121 : double minDist = std::numeric_limits<double>::max();
122 : const L* best = nullptr;
123 2975 : for (const Named* o : lanes) {
124 : const L* cand = static_cast<const L*>(o);
125 2106 : if (!cand->allowsVehicleClass(vClass)) {
126 104 : continue;
127 : }
128 2002 : double dist = cand->getShape().distance2D(pos);
129 2002 : if (dist < minDist) {
130 : minDist = dist;
131 : best = cand;
132 : }
133 : }
134 869 : return best;
135 869 : }
136 :
137 : /// @brief find closest junction taz given the closest edge
138 373 : const E* getJunctionTaz(const Position& pos, const E* closestEdge, SUMOVehicleClass vClass, bool isFrom) {
139 373 : if (closestEdge == nullptr) {
140 : return nullptr;
141 : } else {
142 : const N* fromJunction = closestEdge->getFromJunction();
143 : const N* toJunction = closestEdge->getToJunction();
144 373 : const bool fromCloser = (fromJunction->getPosition().distanceSquaredTo2D(pos) <
145 245 : toJunction->getPosition().distanceSquaredTo2D(pos));
146 373 : const E* fromSource = retrieveEdge(fromJunction->getID() + "-source");
147 373 : const E* fromSink = retrieveEdge(fromJunction->getID() + "-sink");
148 373 : const E* toSource = retrieveEdge(toJunction->getID() + "-source");
149 373 : const E* toSink = retrieveEdge(toJunction->getID() + "-sink");
150 373 : if (fromSource == nullptr || fromSink == nullptr) {
151 0 : myErrorOutput->inform("Junction-taz '" + fromJunction->getID() + "' not found when mapping position " + toString(pos) + "." + JUNCTION_TAZ_MISSING_HELP);
152 0 : return nullptr;
153 : }
154 373 : if (toSource == nullptr || toSink == nullptr) {
155 0 : myErrorOutput->inform("Junction-taz '" + toJunction->getID() + "' not found when mapping position " + toString(pos) + "." + JUNCTION_TAZ_MISSING_HELP);
156 0 : return nullptr;
157 : }
158 373 : const bool fromPossible = isFrom ? fromSource->getSuccessors(vClass).size() > 0 : fromSink->getPredecessors().size() > 0;
159 373 : const bool toPossible = isFrom ? toSource->getSuccessors(vClass).size() > 0 : toSink->getPredecessors().size() > 0;
160 : //std::cout << "getJunctionTaz pos=" << pos << " isFrom=" << isFrom << " closest=" << closestEdge->getID() << " fromPossible=" << fromPossible << " toPossible=" << toPossible << "\n";
161 373 : if (fromCloser && fromPossible) {
162 : // return closest if possible
163 185 : return isFrom ? fromSource : fromSink;
164 208 : } else if (!fromCloser && toPossible) {
165 : // return closest if possible
166 395 : return isFrom ? toSource : toSink;
167 : } else {
168 : // return possible
169 7 : if (fromPossible) {
170 0 : return isFrom ? fromSource : fromSink;
171 : } else {
172 14 : return isFrom ? toSource : toSink;
173 : }
174 : }
175 : }
176 : }
177 :
178 : /// @brief find closest junction taz given the closest edge
179 69 : const E* getTaz(const Position& pos, const E* closestEdge, bool isFrom) {
180 69 : if (closestEdge == nullptr) {
181 : return nullptr;
182 : } else {
183 : std::vector<const E*> cands;
184 69 : if (isFrom) {
185 144 : for (const E* e : closestEdge->getPredecessors()) {
186 106 : if (e->isTazConnector()) {
187 75 : cands.push_back(e);
188 : }
189 : }
190 : } else {
191 89 : for (const E* e : closestEdge->getSuccessors()) {
192 58 : if (e->isTazConnector()) {
193 27 : cands.push_back(e);
194 : }
195 : }
196 : }
197 69 : if (cands.size() == 0) {
198 60 : myErrorOutput->inform("Taz for edge '" + closestEdge->getID() + "' not found when mapping position " + toString(pos) + "." + JUNCTION_TAZ_MISSING_HELP);
199 15 : return nullptr;
200 : }
201 54 : if (cands.size() > 1) {
202 16 : return cands[RandHelper::rand((int)cands.size(), getRNG())];
203 : } else {
204 38 : return cands.front();
205 : }
206 69 : }
207 : }
208 :
209 16 : virtual SumoRNG* getRNG() {
210 16 : return nullptr;
211 : }
212 :
213 : virtual void initLaneTree(NamedRTree* tree) = 0;
214 :
215 : virtual E* retrieveEdge(const std::string& id) = 0;
216 :
217 : bool myMapMatchTAZ;
218 :
219 : private:
220 : /// @brief initialize lane-RTree
221 869 : NamedRTree* getLaneTree() {
222 869 : if (myLaneTree == nullptr) {
223 200 : myLaneTree = new NamedRTree();
224 200 : initLaneTree(myLaneTree);
225 : }
226 869 : return myLaneTree;
227 : }
228 :
229 : /// @brief RTree for finding lanes
230 : NamedRTree* myLaneTree;
231 : bool myMapMatchJunctions;
232 : double myMapMatchingDistance;
233 : MsgHandler* myErrorOutput;
234 :
235 :
236 : private:
237 : /// @brief Invalidated copy constructor
238 : MapMatcher(const MapMatcher& s) = delete;
239 :
240 : /// @brief Invalidated assignment operator
241 : MapMatcher& operator=(const MapMatcher& s) = delete;
242 : };
|
