@@ -320,10 +320,10 @@ void RouteHandler::setLaneletsFromRouteMsg()
320
320
}
321
321
route_lanelets_.clear ();
322
322
preferred_lanelets_.clear ();
323
- const bool is_route_valid = lanelet::utils::route::isRouteValid (*route_ptr_, lanelet_map_ptr_);
324
- if (!is_route_valid) {
325
- return ;
326
- }
323
+ // const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_,
324
+ // lanelet_map_ptr_); if (!is_route_valid) {
325
+ // return;
326
+ // }
327
327
328
328
size_t primitive_size{0 };
329
329
for (const auto & route_section : route_ptr_->segments ) {
@@ -333,11 +333,19 @@ void RouteHandler::setLaneletsFromRouteMsg()
333
333
334
334
for (const auto & route_section : route_ptr_->segments ) {
335
335
for (const auto & primitive : route_section.primitives ) {
336
- const auto id = primitive.id ;
337
- const auto & llt = lanelet_map_ptr_->laneletLayer .get (id);
338
- route_lanelets_.push_back (llt);
339
- if (id == route_section.preferred_primitive .id ) {
340
- preferred_lanelets_.push_back (llt);
336
+ try {
337
+ const auto id = primitive.id ;
338
+ const auto & llt = lanelet_map_ptr_->laneletLayer .get (id);
339
+ route_lanelets_.push_back (llt);
340
+ if (id == route_section.preferred_primitive .id ) {
341
+ preferred_lanelets_.push_back (llt);
342
+ }
343
+ } catch (const std::exception & e) {
344
+ std::cerr
345
+ << e.what ()
346
+ << " . Maybe the loaded route was created on a different Map from the current one. "
347
+ " Try to load the other Route again."
348
+ << std::endl;
341
349
}
342
350
}
343
351
}
@@ -346,15 +354,31 @@ void RouteHandler::setLaneletsFromRouteMsg()
346
354
if (!route_ptr_->segments .empty ()) {
347
355
goal_lanelets_.reserve (route_ptr_->segments .back ().primitives .size ());
348
356
for (const auto & primitive : route_ptr_->segments .back ().primitives ) {
349
- const auto id = primitive.id ;
350
- const auto & llt = lanelet_map_ptr_->laneletLayer .get (id);
351
- goal_lanelets_.push_back (llt);
357
+ try {
358
+ const auto id = primitive.id ;
359
+ const auto & llt = lanelet_map_ptr_->laneletLayer .get (id);
360
+ goal_lanelets_.push_back (llt);
361
+ } catch (const std::exception & e) {
362
+ std::cerr
363
+ << e.what ()
364
+ << " . Maybe the loaded route was created on a different Map from the current one. "
365
+ " Try to load the other Route again."
366
+ << std::endl;
367
+ }
352
368
}
353
369
start_lanelets_.reserve (route_ptr_->segments .front ().primitives .size ());
354
370
for (const auto & primitive : route_ptr_->segments .front ().primitives ) {
355
- const auto id = primitive.id ;
356
- const auto & llt = lanelet_map_ptr_->laneletLayer .get (id);
357
- start_lanelets_.push_back (llt);
371
+ try {
372
+ const auto id = primitive.id ;
373
+ const auto & llt = lanelet_map_ptr_->laneletLayer .get (id);
374
+ start_lanelets_.push_back (llt);
375
+ } catch (const std::exception & e) {
376
+ std::cerr
377
+ << e.what ()
378
+ << " . Maybe the loaded route was created on a different Map from the current one. "
379
+ " Try to load the other Route again."
380
+ << std::endl;
381
+ }
358
382
}
359
383
}
360
384
is_handler_ready_ = true ;
0 commit comments