Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: MIT
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "ad/map/access/Factory.hpp"
10 : :
11 : : #include <algorithm>
12 : : #include "../lane/LaneOperationPrivate.hpp"
13 : : #include "ad/map/access/Logging.hpp"
14 : : #include "ad/map/access/Store.hpp"
15 : : #include "ad/map/landmark/LandmarkOperation.hpp"
16 : : #include "ad/map/lane/LaneOperation.hpp"
17 : : #include "ad/map/point/GeometryOperation.hpp"
18 : : #include "ad/map/point/Transform.hpp"
19 : : #include "ad/map/restriction/ParametricRangeAttributeOperation.hpp"
20 : :
21 : : namespace ad {
22 : : namespace map {
23 : : namespace access {
24 : :
25 : 23503 : bool Factory::add(PartitionId part_id, const lane::LaneId &id, lane::LaneType type, lane::LaneDirection dir)
26 : : {
27 [ + - ]: 23503 : auto insertResult = mStore.lane_map_.insert({id, lane::Lane::Ptr()});
28 : 23503 : auto &lanePtr = insertResult.first->second;
29 [ + - ]: 23503 : if (insertResult.second)
30 : : {
31 [ + - ]: 23503 : lanePtr = std::make_shared<lane::Lane>();
32 : 23503 : lanePtr->id = id;
33 [ + - + - ]: 23503 : mStore.part_lane_map_[part_id].push_back(id);
34 : : }
35 : 23503 : lanePtr->type = type;
36 : 23503 : lanePtr->direction = dir;
37 : 23503 : return insertResult.second;
38 : : }
39 : :
40 : 5 : lane::LaneId Factory::add(PartitionId pid, const point::GeoEdge &left_geo, const point::GeoEdge &right_geo)
41 : : {
42 [ + - ]: 5 : lane::LaneId new_lane_id = getNextLaneId();
43 [ + - + - ]: 5 : if (add(pid, new_lane_id, lane::LaneType::NORMAL, lane::LaneDirection::POSITIVE))
44 : : {
45 [ + - ]: 10 : point::CoordinateTransform cf;
46 : 10 : point::ECEFEdge left_ecef;
47 : 10 : point::ECEFEdge right_ecef;
48 [ + - ]: 5 : cf.convert(left_geo, left_ecef);
49 [ + - ]: 5 : cf.convert(right_geo, right_ecef);
50 [ + - ]: 10 : point::Geometry const left_edge = point::createGeometry(left_ecef, false);
51 [ + - ]: 5 : point::Geometry const right_edge = point::createGeometry(right_ecef, false);
52 [ + - ]: 5 : set(new_lane_id, left_edge, right_edge);
53 : 5 : return new_lane_id;
54 : : }
55 : : else
56 : : {
57 : 0 : return lane::LaneId();
58 : : }
59 : : }
60 : :
61 : 2 : lane::LaneId Factory::add(PartitionId pid,
62 : : const point::ECEFEdge &left_ecef,
63 : : const point::ECEFEdge &right_ecef,
64 : : const lane::LaneId &lane_id_0,
65 : : const lane::LaneId &lane_id_1)
66 : : {
67 [ + - ]: 2 : lane::LaneId new_lane_id = getNextLaneId();
68 [ + - + - ]: 2 : if (add(pid, new_lane_id, lane::LaneType::INTERSECTION, lane::LaneDirection::POSITIVE))
69 : : {
70 [ + - ]: 4 : point::Geometry const left_edge = point::createGeometry(left_ecef, false);
71 [ + - ]: 3 : point::Geometry const right_edge = point::createGeometry(right_ecef, false);
72 [ + - ]: 2 : set(new_lane_id, left_edge, right_edge);
73 [ + - + - : 3 : if (!autoConnect(new_lane_id, lane_id_0) || !autoConnect(new_lane_id, lane_id_1)
+ - ]
74 [ + + + - : 3 : || !autoConnect(lane_id_0, new_lane_id) || !autoConnect(lane_id_1, new_lane_id))
+ - + - -
+ + + ]
75 : : {
76 [ + - ]: 1 : throw std::runtime_error("AutoConnect failed");
77 : : }
78 : 1 : return new_lane_id;
79 : : }
80 : : else
81 : : {
82 : 0 : return lane::LaneId();
83 : : }
84 : : }
85 : :
86 : 22 : bool Factory::addLandmark(PartitionId part_id,
87 : : const landmark::LandmarkId &id,
88 : : const landmark::LandmarkType type,
89 : : const point::ECEFPoint &position,
90 : : const point::ECEFPoint &orientation,
91 : : const point::Geometry &bounding_box)
92 : : {
93 [ + - + - ]: 44 : return add(part_id,
94 : : id,
95 : : type,
96 : : position,
97 : : orientation,
98 : : bounding_box,
99 : : landmark::TrafficLightType::INVALID,
100 : : landmark::TrafficSignType::INVALID,
101 : 44 : "none");
102 : : }
103 : :
104 : 6911 : bool Factory::addTrafficLight(PartitionId part_id,
105 : : const landmark::LandmarkId &id,
106 : : const landmark::TrafficLightType type,
107 : : const point::ECEFPoint &position,
108 : : const point::ECEFPoint &orientation,
109 : : const point::Geometry &bounding_box)
110 : : {
111 [ + - + - ]: 13822 : return add(part_id,
112 : : id,
113 : : landmark::LandmarkType::TRAFFIC_LIGHT,
114 : : position,
115 : : orientation,
116 : : bounding_box,
117 : : type,
118 : : landmark::TrafficSignType::INVALID,
119 : 13822 : "none");
120 : : }
121 : :
122 : 84 : bool Factory::addTrafficSign(PartitionId part_id,
123 : : const landmark::LandmarkId &id,
124 : : const landmark::TrafficSignType type,
125 : : const point::ECEFPoint &position,
126 : : const point::ECEFPoint &orientation,
127 : : const point::Geometry &bounding_box,
128 : : const std::string &text)
129 : : {
130 : 84 : return add(part_id,
131 : : id,
132 : : landmark::LandmarkType::TRAFFIC_SIGN,
133 : : position,
134 : : orientation,
135 : : bounding_box,
136 : : landmark::TrafficLightType::INVALID,
137 : : type,
138 : 84 : text);
139 : : }
140 : :
141 : 7017 : bool Factory::add(PartitionId part_id,
142 : : const landmark::LandmarkId &id,
143 : : const landmark::LandmarkType type,
144 : : const point::ECEFPoint &position,
145 : : const point::ECEFPoint &orientation,
146 : : const point::Geometry &bounding_box,
147 : : const landmark::TrafficLightType traffic_light_type,
148 : : const landmark::TrafficSignType traffic_sign_type,
149 : : const std::string &text)
150 : : {
151 [ + - ]: 7017 : auto insertResult = mStore.landmark_map_.insert({id, landmark::Landmark::Ptr()});
152 : 7017 : auto &landmarkPtr = insertResult.first->second;
153 [ + + ]: 7017 : if (insertResult.second)
154 : : {
155 [ + - ]: 1325 : landmarkPtr = std::make_shared<landmark::Landmark>();
156 : 1325 : landmarkPtr->id = id;
157 [ + - + - ]: 1325 : mStore.part_landmark_map_[part_id].push_back(id);
158 : : }
159 : 7017 : landmarkPtr->type = type;
160 : 7017 : landmarkPtr->position = position;
161 : 7017 : landmarkPtr->orientation = orientation;
162 [ + - ]: 7017 : landmarkPtr->boundingBox = bounding_box;
163 : 7017 : landmarkPtr->trafficLightType = traffic_light_type;
164 : 7017 : landmarkPtr->trafficSignType = traffic_sign_type;
165 [ + - ]: 7017 : landmarkPtr->supplementaryText = text;
166 : : // @todo: Do we need to ensure that we handle only valid objects?
167 : 7017 : return insertResult.second;
168 : : }
169 : :
170 : : //////////////////////////////////////////
171 : : // Atomic Operations -- Add Lane Attribute
172 : :
173 : 118457 : bool Factory::add(const lane::LaneId &id, const lane::ContactLane &contact_lane)
174 : : {
175 [ + - ]: 118457 : auto it = mStore.lane_map_.find(id);
176 [ + + ]: 118457 : if (it != mStore.lane_map_.end())
177 : : {
178 : 118456 : lane::Lane::Ptr lane = it->second;
179 [ + - ]: 118456 : if (lane)
180 : : {
181 [ + + ]: 435287 : for (auto const &existing_contact_lane : lane->contactLanes)
182 : : {
183 [ + - + + ]: 316832 : if (contact_lane == existing_contact_lane)
184 : : {
185 : 1 : return true;
186 : : }
187 : : }
188 [ + - ]: 118455 : lane->contactLanes.push_back(contact_lane);
189 : 118455 : return true;
190 : : }
191 : : }
192 [ + - + - ]: 2 : access::getLogger()->error("Cannot add contact lane of lane. {}", id);
193 : 1 : return false;
194 : : }
195 : :
196 : 2 : bool Factory::add(const lane::LaneId &id, const lane::ContactLaneList &contact_lanes)
197 : : {
198 [ + + + - ]: 3 : for (auto contact_lane : contact_lanes)
199 : : {
200 [ + - + + ]: 2 : if (!add(id, contact_lane))
201 : : {
202 [ + - + - ]: 2 : access::getLogger()->error("Cannot add contact lanes of lane. {}", id);
203 : 1 : return false;
204 : : }
205 : : }
206 : 1 : return true;
207 : : }
208 : :
209 : 35259 : bool Factory::add(const lane::LaneId &id, const restriction::SpeedLimit &speedLimit)
210 : : {
211 [ + - ]: 35259 : auto it = mStore.lane_map_.find(id);
212 [ + + ]: 35259 : if (it != mStore.lane_map_.end())
213 : : {
214 : 35258 : lane::Lane::Ptr lane = it->second;
215 [ + - ]: 35258 : if (lane)
216 : : {
217 [ + - + + ]: 35258 : if (restriction::doesRangeAttributeOverlap(lane->speedLimits, speedLimit))
218 : : {
219 [ + - + - ]: 2 : access::getLogger()->warn("Lane para-speed overlaps existing value!? {}, {}", id, speedLimit);
220 : : }
221 [ + - ]: 35258 : restriction::insertRangeAttribute(lane->speedLimits, speedLimit);
222 : 35258 : return true;
223 : : }
224 : : }
225 [ + - + - ]: 2 : access::getLogger()->error("Cannot add restrictions of lane. {}", id);
226 : 1 : return false;
227 : : }
228 : :
229 : 85 : bool Factory::add(const lane::LaneId &id, const landmark::LandmarkId &landmarkId)
230 : : {
231 [ + - - + ]: 85 : if (!isValid(landmarkId))
232 : : {
233 [ # # # # ]: 0 : access::getLogger()->error("Cannot add landmark with invalid id. {}", landmarkId);
234 : 0 : return false;
235 : : }
236 [ + - ]: 85 : auto it = mStore.lane_map_.find(id);
237 [ + + ]: 85 : if (it != mStore.lane_map_.end())
238 : : {
239 : 84 : lane::Lane::Ptr lane = it->second;
240 [ + - ]: 84 : if (lane)
241 : : {
242 [ + + ]: 2713 : for (auto existing_visible_landmark : lane->visibleLandmarks)
243 : : {
244 [ + - - + ]: 2629 : if (landmarkId == existing_visible_landmark)
245 : : {
246 : 0 : return true;
247 : : }
248 : : }
249 [ + - ]: 84 : lane->visibleLandmarks.push_back(landmarkId);
250 : 84 : return true;
251 : : }
252 : : }
253 [ + - + - ]: 2 : access::getLogger()->error("Cannot add landmark of lane. {}", id);
254 : 1 : return false;
255 : : }
256 : :
257 : 3 : bool Factory::add(const lane::LaneId &id, const restriction::Restriction &restriction, bool andx)
258 : : {
259 [ + - ]: 3 : auto it = mStore.lane_map_.find(id);
260 [ + + ]: 3 : if (it != mStore.lane_map_.end())
261 : : {
262 : 2 : lane::Lane::Ptr lane = it->second;
263 [ + - ]: 2 : if (lane)
264 : : {
265 [ + + ]: 2 : if (andx)
266 : : {
267 [ + - ]: 1 : lane->restrictions.conjunctions.push_back(restriction);
268 : : }
269 : : else
270 : : {
271 [ + - ]: 1 : lane->restrictions.disjunctions.push_back(restriction);
272 : : }
273 : 2 : return true;
274 : : }
275 : : }
276 [ + - + - ]: 2 : access::getLogger()->error("Cannot add restrictions of lane. {}", id);
277 : 1 : return false;
278 : : }
279 : :
280 : 112938 : bool Factory::add(const lane::LaneId &id_from,
281 : : const lane::LaneId &id_to,
282 : : const lane::ContactLocation location,
283 : : const lane::ContactTypeList &types,
284 : : const restriction::Restrictions &restrs)
285 : : {
286 [ + - + + ]: 112938 : if (std::find(types.begin(), types.end(), lane::ContactType::TRAFFIC_LIGHT) != types.end())
287 : : {
288 [ + - + - ]: 2 : access::getLogger()->error("Trying to add traffic light contact without traffic light id. {}", id_to);
289 : 1 : return false;
290 : : }
291 : :
292 [ + - ]: 225874 : lane::ContactLane contactLane;
293 : 112937 : contactLane.toLane = id_to;
294 : 112937 : contactLane.location = location;
295 [ + - ]: 112937 : contactLane.types = types;
296 [ + - ]: 112937 : contactLane.restrictions = restrs;
297 : 112937 : contactLane.trafficLightId = landmark::LandmarkId();
298 [ + - ]: 112937 : return add(id_from, contactLane);
299 : : }
300 : :
301 : 5519 : bool Factory::add(const lane::LaneId &id_from,
302 : : const lane::LaneId &id_to,
303 : : const lane::ContactLocation location,
304 : : const lane::ContactTypeList &types,
305 : : const restriction::Restrictions &restrs,
306 : : const landmark::LandmarkId &traffic_light)
307 : : {
308 [ + - - + ]: 5519 : if (!isValid(traffic_light))
309 : : {
310 [ # # # # ]: 0 : access::getLogger()->error("Cannot add contact with invalid traffic light id. {}", traffic_light);
311 : 0 : return false;
312 : : }
313 : :
314 [ + - + + ]: 5519 : if (std::find(types.begin(), types.end(), lane::ContactType::TRAFFIC_LIGHT) == types.end())
315 : : {
316 [ + - + - ]: 2 : access::getLogger()->error("Cannot add traffic light contact due to wrong type. {}", traffic_light);
317 : 1 : return false;
318 : : }
319 : :
320 [ + - ]: 11036 : lane::ContactLane contactLane;
321 : 5518 : contactLane.toLane = id_to;
322 : 5518 : contactLane.location = location;
323 [ + - ]: 5518 : contactLane.types = types;
324 [ + - ]: 5518 : contactLane.restrictions = restrs;
325 : 5518 : contactLane.trafficLightId = traffic_light;
326 [ + - ]: 5518 : return add(id_from, contactLane);
327 : : }
328 : :
329 : : //////////////////////////////////////////
330 : : // Atomic Operations -- Set Lane Attribute
331 : :
332 : 2 : bool Factory::set(const lane::LaneId &id, lane::LaneDirection direction)
333 : : {
334 [ + - ]: 2 : auto it = mStore.lane_map_.find(id);
335 [ + + ]: 2 : if (it != mStore.lane_map_.end())
336 : : {
337 : 1 : lane::Lane::Ptr lane = it->second;
338 [ + - ]: 1 : if (lane)
339 : : {
340 : 1 : lane->direction = direction;
341 : 1 : return true;
342 : : }
343 : : }
344 [ + - + - ]: 2 : access::getLogger()->error("Cannot set direction of lane. {}", id);
345 : 1 : return false;
346 : : }
347 : :
348 : 2 : bool Factory::set(const lane::LaneId &id, lane::LaneType type)
349 : : {
350 [ + - ]: 2 : auto it = mStore.lane_map_.find(id);
351 [ + + ]: 2 : if (it != mStore.lane_map_.end())
352 : : {
353 : 1 : lane::Lane::Ptr lane = it->second;
354 [ + - ]: 1 : if (lane)
355 : : {
356 : 1 : lane->type = type;
357 : 1 : return true;
358 : : }
359 : : }
360 [ + - + - ]: 2 : access::getLogger()->error("Cannot set type of lane. {}", id);
361 : 1 : return false;
362 : : }
363 : :
364 : 2 : bool Factory::set(const lane::LaneId &id, lane::ComplianceVersion compliance_ver)
365 : : {
366 [ + - ]: 2 : auto it = mStore.lane_map_.find(id);
367 [ + + ]: 2 : if (it != mStore.lane_map_.end())
368 : : {
369 : 1 : lane::Lane::Ptr lane = it->second;
370 [ + - ]: 1 : if (lane)
371 : : {
372 : 1 : lane->complianceVersion = compliance_ver;
373 : 1 : return true;
374 : : }
375 : : }
376 [ + - + - ]: 2 : access::getLogger()->error("Cannot set compliance of lane. {}", id);
377 : 1 : return false;
378 : : }
379 : :
380 : 23505 : bool Factory::set(const lane::LaneId &id, const point::Geometry &edge_left, const point::Geometry &edge_right)
381 : : {
382 [ + - ]: 23505 : auto it = mStore.lane_map_.find(id);
383 [ + + ]: 23505 : if (it != mStore.lane_map_.end())
384 : : {
385 : 23504 : lane::Lane::Ptr lane = it->second;
386 [ + - ]: 23504 : if (lane)
387 : : {
388 [ + - ]: 23504 : lane->edgeLeft = edge_left;
389 [ + - ]: 23504 : lane->edgeRight = edge_right;
390 [ + - ]: 23504 : lane->boundingSphere = point::calcBoundingSphere(edge_left, edge_right);
391 [ + - ]: 23504 : lane::updateLaneLengths(*lane);
392 : 23504 : return true;
393 : : }
394 : : }
395 [ + - + - ]: 2 : access::getLogger()->error("Cannot set edges of lane. {}", id);
396 : 1 : return false;
397 : : }
398 : :
399 : 3 : bool Factory::set(const lane::LaneId &id, const physics::Speed &maxSpeed)
400 : : {
401 [ + - ]: 3 : auto it = mStore.lane_map_.find(id);
402 [ + + ]: 3 : if (it != mStore.lane_map_.end())
403 : : {
404 : 1 : lane::Lane::Ptr lane = it->second;
405 [ + - ]: 1 : if (lane)
406 : : {
407 : 1 : lane->speedLimits.clear();
408 [ + - ]: 1 : restriction::SpeedLimit speedLimit;
409 : 1 : speedLimit.lanePiece.minimum = physics::ParametricValue(0.);
410 : 1 : speedLimit.lanePiece.maximum = physics::ParametricValue(1.);
411 : 1 : speedLimit.speedLimit = maxSpeed;
412 [ + - ]: 1 : lane->speedLimits.push_back(speedLimit);
413 : 1 : return true;
414 : : }
415 : : }
416 [ + - + - ]: 4 : access::getLogger()->error("Cannot set speed limit for the. {}", id);
417 : 2 : return false;
418 : : }
419 : :
420 : 22 : bool Factory::set(const lane::LaneId &id, const restriction::Restrictions &restrictions)
421 : : {
422 [ + - ]: 22 : auto it = mStore.lane_map_.find(id);
423 [ + + ]: 22 : if (it != mStore.lane_map_.end())
424 : : {
425 : 21 : lane::Lane::Ptr lane = it->second;
426 [ + - ]: 21 : if (lane)
427 : : {
428 [ + - ]: 21 : lane->restrictions = restrictions;
429 : 21 : return true;
430 : : }
431 : : }
432 [ + - + - ]: 2 : access::getLogger()->error("Cannot set restrictions of lane. {}", id);
433 : 1 : return false;
434 : : }
435 : :
436 : 2 : bool Factory::set(const lane::LaneId &id, const landmark::LandmarkIdList &landmarks)
437 : : {
438 [ + - ]: 2 : auto it = mStore.lane_map_.find(id);
439 [ + + ]: 2 : if (it != mStore.lane_map_.end())
440 : : {
441 : 1 : lane::Lane::Ptr lane = it->second;
442 [ + - ]: 1 : if (lane)
443 : : {
444 [ + - ]: 1 : lane->visibleLandmarks = landmarks;
445 : 1 : return true;
446 : : }
447 : : }
448 [ + - + - ]: 2 : access::getLogger()->error("Cannot set bounding sphere of lane. {}", id);
449 : 1 : return false;
450 : : }
451 : :
452 : : //////////////////////////////////////////
453 : : // Atomic Operations -- Set Map Meta Data
454 : :
455 : 67 : bool Factory::set(const TrafficType &traffic_type)
456 : : {
457 [ + - ]: 67 : if (traffic_type != TrafficType::INVALID)
458 : : {
459 : 67 : mStore.meta_data_.trafficType = traffic_type;
460 : 67 : return true;
461 : : }
462 : :
463 : 0 : return false;
464 : : }
465 : :
466 : : /////////////////////////////////////
467 : : // Atomic operations -- remove things
468 : :
469 : 3 : bool Factory::deleteLane(lane::LaneId id)
470 : : {
471 [ + + ]: 3 : if (lane::isValid(id))
472 : : {
473 : 2 : auto const erased = mStore.lane_map_.erase(id);
474 [ + + ]: 2 : if (erased > 0u)
475 : : {
476 : 1 : bool removed_from_part_map = false;
477 [ + - + - ]: 1 : for (auto part_id_and_lane_ids : mStore.part_lane_map_)
478 : : {
479 [ + - ]: 1 : auto const removeIter = std::remove(part_id_and_lane_ids.second.begin(), part_id_and_lane_ids.second.end(), id);
480 [ + - ]: 1 : if (removeIter != part_id_and_lane_ids.second.end())
481 : : {
482 [ + - ]: 1 : part_id_and_lane_ids.second.erase(removeIter, part_id_and_lane_ids.second.end());
483 : 1 : removed_from_part_map = true;
484 : 1 : break;
485 : : }
486 : : }
487 [ - + ]: 1 : if (!removed_from_part_map)
488 : : {
489 [ # # ]: 0 : access::getLogger()->error("Lane not in the Partitions?. {}", id);
490 : : }
491 : 1 : return true;
492 : : }
493 : : else
494 : : {
495 [ + - ]: 2 : access::getLogger()->warn("Lane is not in the Store. {}", id);
496 : : }
497 : : }
498 : : else
499 : : {
500 [ + - ]: 1 : access::getLogger()->error("Invalid lane identifier passed to Factory::deleteLane()");
501 : : }
502 : 2 : return false;
503 : : }
504 : :
505 : 2 : bool Factory::deleteContacts(lane::LaneId id, lane::LaneId to_id)
506 : : {
507 [ + + + - : 2 : if (lane::isValid(id) && lane::isValid(to_id))
+ + ]
508 : : {
509 [ + - ]: 1 : auto it = mStore.lane_map_.find(id);
510 [ + - ]: 1 : if (it != mStore.lane_map_.end())
511 : : {
512 : 1 : lane::Lane::Ptr lane = it->second;
513 [ + - ]: 1 : if (lane)
514 : : {
515 : 1 : lane::ContactLaneList &cls = lane->contactLanes;
516 : : cls.erase(
517 [ + - + - ]: 3 : std::remove_if(cls.begin(), cls.end(), [to_id](const lane::ContactLane &cl) { return cl.toLane == to_id; }));
518 : 1 : return true;
519 : : }
520 : : }
521 : : }
522 : : else
523 : : {
524 [ + - ]: 1 : access::getLogger()->error("Invalid lane identifier passed to Factory::deleteContacts()");
525 : : }
526 : 1 : return false;
527 : : }
528 : :
529 : 2 : bool Factory::deleteLandmark(landmark::LandmarkId id)
530 : : {
531 [ + - ]: 2 : if (landmark::isValid(id))
532 : : {
533 : 2 : auto const erased = mStore.landmark_map_.erase(id);
534 [ + + ]: 2 : if (erased > 0u)
535 : : {
536 : 1 : bool removed_from_part_map = false;
537 [ + - + - ]: 1 : for (auto part_id_and_landmark_ids : mStore.part_landmark_map_)
538 : : {
539 : : auto const removeIter
540 [ + - ]: 1 : = std::remove(part_id_and_landmark_ids.second.begin(), part_id_and_landmark_ids.second.end(), id);
541 [ + - ]: 1 : if (removeIter != part_id_and_landmark_ids.second.end())
542 : : {
543 [ + - ]: 1 : part_id_and_landmark_ids.second.erase(removeIter, part_id_and_landmark_ids.second.end());
544 : 1 : removed_from_part_map = true;
545 : 1 : break;
546 : : }
547 : : }
548 [ - + ]: 1 : if (!removed_from_part_map)
549 : : {
550 [ # # ]: 0 : access::getLogger()->error("Landmark not in a partition. {}", id);
551 : : }
552 : 1 : return true;
553 : : }
554 : : else
555 : : {
556 [ + - ]: 2 : access::getLogger()->warn("Landmark is not in the Store. {}", id);
557 : : }
558 : : }
559 : : else
560 : : {
561 [ # # ]: 0 : access::getLogger()->error("Invalid landmark identifier passed to Factory::deleteLandmark()");
562 : : }
563 : 1 : return false;
564 : : }
565 : :
566 : : ////////////////
567 : : // Other Methods
568 : :
569 : 11 : bool Factory::autoConnect(lane::LaneId from_id, lane::LaneId to_id)
570 : : {
571 : 11 : bool ok = false;
572 [ + + + - : 11 : if (lane::isValid(from_id) && lane::isValid(to_id))
+ + ]
573 : : {
574 [ + - ]: 20 : lane::Lane::ConstPtr from_lane = mStore.getLanePtr(from_id);
575 [ + + ]: 10 : if (from_lane)
576 : : {
577 [ + - ]: 18 : lane::Lane::ConstPtr to_lane = mStore.getLanePtr(to_id);
578 [ + + ]: 9 : if (to_lane)
579 : : {
580 [ + - + - ]: 8 : if (lane::getContactLocation(*from_lane, to_id) == lane::ContactLocation::INVALID)
581 : : {
582 : : static constexpr auto pred = lane::ContactLocation::PREDECESSOR;
583 : : static constexpr auto succ = lane::ContactLocation::SUCCESSOR;
584 [ + - ]: 16 : lane::ContactTypeList types = {lane::ContactType::FREE};
585 : 16 : restriction::Restrictions restrs;
586 [ + - + - : 8 : if (lane::getStartPoint(*from_lane) == lane::getStartPoint(*to_lane))
+ - + + ]
587 : : {
588 [ + - ]: 1 : ok = add(from_id, to_id, pred, types, restrs);
589 : : }
590 [ + - + - : 7 : else if (lane::getStartPoint(*from_lane) == lane::getEndPoint(*to_lane))
+ - + + ]
591 : : {
592 [ + - ]: 2 : ok = add(from_id, to_id, pred, types, restrs);
593 : : }
594 [ + - + - : 5 : else if (lane::getEndPoint(*from_lane) == lane::getStartPoint(*to_lane))
+ - + + ]
595 : : {
596 [ + - ]: 2 : ok = add(from_id, to_id, succ, types, restrs);
597 : : }
598 [ + - + - : 3 : else if (lane::getEndPoint(*from_lane) == lane::getEndPoint(*to_lane))
+ - + + ]
599 : : {
600 [ + - ]: 1 : ok = add(from_id, to_id, succ, types, restrs);
601 : : }
602 : : }
603 : : }
604 : : else
605 : : {
606 [ + - + - ]: 1 : access::getLogger()->error("Non-existing to-lane Factory::autoConnect()");
607 : : }
608 : : }
609 : : else
610 : : {
611 [ + - + - ]: 1 : access::getLogger()->error("Non-existing from-lane Factory::autoConnect()");
612 : : }
613 : : }
614 : : else
615 : : {
616 [ + - ]: 1 : access::getLogger()->error("Invalid lane identifier passed to Factory::autoConnect()");
617 : : }
618 : 11 : return ok;
619 : : }
620 : :
621 : : /////////////
622 : : // Aux Methods
623 : :
624 : 7 : lane::LaneId Factory::getNextLaneId() const
625 : : {
626 : 7 : lane::LaneId new_lane_id(0);
627 [ + + ]: 43 : for (auto it : mStore.lane_map_)
628 : : {
629 [ + - ]: 36 : new_lane_id = std::max(new_lane_id, it.second->id);
630 : : }
631 [ + - ]: 7 : new_lane_id += lane::LaneId(1);
632 : 7 : return new_lane_id;
633 : : }
634 : :
635 : : } // namespace access
636 : : } // namespace map
637 : : } // namespace ad
|
