Skip to content

Commit b06513c

Browse files
committed
Revize behavior planner to work with dynamic lanelets
Signed-off-by: Barış Zeren <bzeren1819@gmail.com>
1 parent d408e15 commit b06513c

File tree

4 files changed

+54
-16
lines changed

4 files changed

+54
-16
lines changed

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -353,6 +353,7 @@ void BehaviorPathPlannerNode::run()
353353
// update map
354354
if (map_ptr) {
355355
planner_data_->route_handler->setMap(*map_ptr);
356+
planner_manager_->resetRootLanelet(planner_data_);
356357
}
357358

358359
std::unique_lock<std::mutex> lk_manager(mutex_manager_); // for planner_manager_

planning/behavior_path_planner_common/src/utils/utils.cpp

+13
Original file line numberDiff line numberDiff line change
@@ -503,6 +503,19 @@ bool isEgoOutOfRoute(
503503
if (utils::isInLanelets(goal_pose, shoulder_lanes)) {
504504
return !lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &goal_lane);
505505
}
506+
lanelet::LaneletMapPtr map = route_handler->getLaneletMapPtr();
507+
lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map);
508+
lanelet::ConstLanelets route_lanelets = route_handler->getRouteLanelets();
509+
for (auto it = route_lanelets.rbegin(); it != route_lanelets.rend(); ++it) {
510+
for (auto it2 = all_lanelets.rbegin(); it2 != all_lanelets.rend(); ++it2) {
511+
if ((*it).id() == (*it2).id()) {
512+
// std::cout << "Route lanelet id: " << (*it).id() << std::endl;
513+
// select the lanelet goal lanelet
514+
goal_lane = *it2;
515+
return false;
516+
}
517+
}
518+
}
506519
return !route_handler->getGoalLanelet(&goal_lane);
507520
});
508521
if (is_failed_getting_lanelet) {

planning/route_handler/include/route_handler/route_handler.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -353,6 +353,7 @@ class RouteHandler
353353
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const;
354354
lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const;
355355
bool isPreferredLane(const lanelet::ConstLanelet & lanelet) const;
356+
lanelet::ConstLanelets getRouteLanelets() const;
356357

357358
private:
358359
// MUST
@@ -398,7 +399,6 @@ class RouteHandler
398399
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const;
399400
bool getLeftLaneletWithinRoute(
400401
const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const;
401-
lanelet::ConstLanelets getRouteLanelets() const;
402402
lanelet::ConstLanelets getLaneletSequenceUpTo(
403403
const lanelet::ConstLanelet & lanelet,
404404
const double min_length = std::numeric_limits<double>::max(),

planning/route_handler/src/route_handler.cpp

+39-15
Original file line numberDiff line numberDiff line change
@@ -320,10 +320,10 @@ void RouteHandler::setLaneletsFromRouteMsg()
320320
}
321321
route_lanelets_.clear();
322322
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+
// }
327327

328328
size_t primitive_size{0};
329329
for (const auto & route_section : route_ptr_->segments) {
@@ -333,11 +333,19 @@ void RouteHandler::setLaneletsFromRouteMsg()
333333

334334
for (const auto & route_section : route_ptr_->segments) {
335335
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;
341349
}
342350
}
343351
}
@@ -346,15 +354,31 @@ void RouteHandler::setLaneletsFromRouteMsg()
346354
if (!route_ptr_->segments.empty()) {
347355
goal_lanelets_.reserve(route_ptr_->segments.back().primitives.size());
348356
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+
}
352368
}
353369
start_lanelets_.reserve(route_ptr_->segments.front().primitives.size());
354370
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+
}
358382
}
359383
}
360384
is_handler_ready_ = true;

0 commit comments

Comments
 (0)