fix(behavior_path_planner): add guard to extend lane

[kosuke55]

Description

fix

#0  0x00007f65503de8b4 in ?? () from /opt/ros/humble/lib/x86_64-linux-gnu/liblanelet2_routing.so.1
[Current thread is 1 (Thread 0x7f65307f8640 (LWP 2786369))]
(gdb) bt
#0  0x00007f65503de8b4 in ?? () from /opt/ros/humble/lib/x86_64-linux-gnu/liblanelet2_routing.so.1
#1  0x00007f65503fec0c in ?? () from /opt/ros/humble/lib/x86_64-linux-gnu/liblanelet2_routing.so.1
#2  0x00007f65503fef50 in lanelet::routing::RoutingGraph::previous(lanelet::ConstLanelet const&, bool) const () from /opt/ros/humble/lib/x86_64-linux-gnu/liblanelet2_routing.so.1
#3  0x00007f6550606547 in route_handler::RouteHandler::getPreviousLanelets(lanelet::ConstLanelet const&) const () from /home/kosuke55/pilot-auto/install/route_handler/lib/libroute_handler.so
#4  0x00007f652cb4b548 in behavior_path_planner::utils::extendPrevLane (route_handler=std::shared_ptr<route_handler::RouteHandler> (use count 2, weak count 0) = {...}, lanes=std::vector of length 0, capacity 0) at /usr/include/c++/11/bits/stl_iterator.h:1027
#5  0x00007f652cb4bc6e in behavior_path_planner::utils::getCurrentLanesFromPath (path=..., planner_data=std::shared_ptr<const behavior_path_planner::PlannerData> (use count 22, weak count 0) = {...}) at ../../src/autoware/universe/planning/behavior_path_planner/src/utils/utils.cpp:3096
#6  0x00007f652cb0de74 in behavior_path_planner::NormalLaneChange::getCurrentLanes (this=0x7f64d8055270) at ../../src/autoware/universe/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp:406
#7  behavior_path_planner::NormalLaneChange::isLaneChangeRequired (this=0x7f64d8055270) at ../../src/autoware/universe/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp:117
#8  0x00007f652c956ffa in behavior_path_planner::PlannerManager::getRequestModules (this=0x7f65041a4ee0, previous_module_output=...) at /usr/include/c++/11/bits/shared_ptr_base.h:1295
#9  0x00007f652c95eeab in operator() (__closure=0x7f65307f68e0) at ../../src/autoware/universe/planning/behavior_path_planner/src/planner_manager.cpp:95
#10 0x00007f652c95fc18 in behavior_path_planner::PlannerManager::run (this=this@entry=0x7f65041a4ee0, data=std::shared_ptr<behavior_path_planner::PlannerData> (use count 22, weak count 0) = {...}) at ../../src/autoware/universe/planning/behavior_path_planner/src/planner_manager.cpp:55
#11 0x00007f652c995f9e in behavior_path_planner::BehaviorPathPlannerNode::run (this=0x7f65040478d0) at /usr/include/c++/11/bits/shared_ptr_base.h:1295
#12 0x00007f652c99b1d5 in std::__invoke_impl<void, void (behavior_path_planner::BehaviorPathPlannerNode::*&)(), behavior_path_planner::BehaviorPathPlannerNode*&> (__t=@0x7f6504114410: 0x7f65040478d0,
    __f=@0x7f6504114400: (void (behavior_path_planner::BehaviorPathPlannerNode::*)(behavior_path_planner::BehaviorPathPlannerNode * const)) 0x7f652c995b70 <behavior_path_planner::BehaviorPathPlannerNode::run()>) at /usr/include/c++/11/bits/invoke.h:71
#13 std::__invoke<void (behavior_path_planner::BehaviorPathPlannerNode::*&)(), behavior_path_planner::BehaviorPathPlannerNode*&> (__fn=@0x7f6504114400: (void (behavior_path_planner::BehaviorPathPlannerNode::*)(behavior_path_planner::BehaviorPathPlannerNode * const)) 0x7f652c995b70 <behavior_path_planner::BehaviorPathPlannerNode::run()>) at /usr/include/c++/11/bits/invoke.h:96
#14 std::_Bind<void (behavior_path_planner::BehaviorPathPlannerNode::*(behavior_path_planner::BehaviorPathPlannerNode*))()>::__call<void, , 0ul>(std::tuple<>&&, std::_Index_tuple<0ul>) (__args=..., this=0x7f6504114400) at /usr/include/c++/11/functional:420
#15 std::_Bind<void (behavior_path_planner::BehaviorPathPlannerNode::*(behavior_path_planner::BehaviorPathPlannerNode*))()>::operator()<, void>() (this=0x7f6504114400) at /usr/include/c++/11/functional:503
#16 rclcpp::GenericTimer<std::_Bind<void (behavior_path_planner::BehaviorPathPlannerNode::*(behavior_path_planner::BehaviorPathPlannerNode*))()>, (void*)0>::execute_callback_delegate<std::_Bind<void (behavior_path_planner::BehaviorPathPlannerNode::*(behavior_path_planner::BehaviorPathPlannerNode*))()>, (void*)0>() (this=0x7f65041143d0)
    at ../../install/rclcpp/include/rclcpp/rclcpp/timer.hpp:244
#17 rclcpp::GenericTimer<std::_Bind<void (behavior_path_planner::BehaviorPathPlannerNode::*(behavior_path_planner::BehaviorPathPlannerNode*))()>, (void*)0>::execute_callback() (this=0x7f65041143d0) at ../../install/rclcpp/include/rclcpp/rclcpp/timer.hpp:230
#18 0x00007f655cf5effe in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) () from /home/kosuke55/pilot-auto/install/rclcpp/lib/librclcpp.so
#19 0x00007f655cf65432 in rclcpp::executors::MultiThreadedExecutor::run(unsigned long) () from /home/kosuke55/pilot-auto/install/rclcpp/lib/librclcpp.so
#20 0x00007f655cadc253 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#21 0x00007f655c694ac3 in start_thread (arg=<optimized out>) at ./nptl/pthread_create.c:442
#22 0x00007f655c726a40 in clone3 () at ../sysdeps/unix/sysv/linux/x86_64/clone3.S:81
(gdb) f 2
#2  0x00007f65503fef50 in lanelet::routing::RoutingGraph::previous(lanelet::ConstLanelet const&, bool) const () from /opt/ros/humble/lib/x86_64-linux-gnu/liblanelet2_routing.so.1
(gdb) f 3
#3  0x00007f6550606547 in route_handler::RouteHandler::getPreviousLanelets(lanelet::ConstLanelet const&) const () from /home/kosuke55/pilot-auto/install/route_handler/lib/libroute_handler.so
(gdb) f 4
#4  0x00007f652cb4b548 in behavior_path_planner::utils::extendPrevLane (route_handler=std::shared_ptr<route_handler::RouteHandler> (use count 2, weak count 0) = {...}, lanes=std::vector of length 0, capacity 0) at /usr/include/c++/11/bits/stl_iterator.h:1027
1027          __normal_iterator(const _Iterator& __i) _GLIBCXX_NOEXCEPT
(gdb) f 5
#5  0x00007f652cb4bc6e in behavior_path_planner::utils::getCurrentLanesFromPath (path=..., planner_data=std::shared_ptr<const behavior_path_planner::PlannerData> (use count 22, weak count 0) = {...}) at ../../src/autoware/universe/planning/behavior_path_planner/src/utils/utils.cpp:3096
3096        const auto extended_lanes = extendPrevLane(route_handler, current_lanes);
(gdb) l
3091          return std::find(front_lane_ids.begin(), front_lane_ids.end(), lane.id()) !=
3092                 front_lane_ids.end();
3093        });
3094      };
3095      while (!have_front_lanes(current_lanes)) {
3096        const auto extended_lanes = extendPrevLane(route_handler, current_lanes);
3097        if (extended_lanes.size() == current_lanes.size()) break;
3098        current_lanes = extended_lanes;
3099      }
3100
(gdb) p current_lanes
$1 = std::vector of length 0, capacity 0

Tests performed

psim

evaluator_description: fix/exnted_lane
2023/10/25 https://evaluation.tier4.jp/evaluation/reports/6e99e6a9-801d-5a25-9913-db65feda6a89/?project_id=prd_jt

Not applicable.

Effects on system behavior

Not applicable.

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.