From 28552895696413699acb4268bb26a8942c11f4cb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bar=C4=B1=C5=9F=20Zeren?= Date: Fri, 9 Feb 2024 12:35:05 +0300 Subject: [PATCH 01/14] Revize behavior planner to work with dynamic lanelets MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Barış Zeren --- .../autoware/route_handler/route_handler.hpp | 12 +++++ .../src/route_handler.cpp | 54 +++++++++++++------ .../src/behavior_path_planner_node.cpp | 1 + 3 files changed, 52 insertions(+), 15 deletions(-) diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index 13e13b642af1f..5467b48674d5f 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -325,6 +325,7 @@ class RouteHandler * @return vector of shoulder lanelets intersecting with given pose. */ lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const; + lanelet::ConstLanelets getRouteLanelets() const; private: // MUST @@ -355,6 +356,17 @@ class RouteHandler lanelet::ConstLanelets getMainLanelets(const lanelet::ConstLanelets & path_lanelets) const; // for lanelet + bool isBijectiveConnection( + const lanelet::ConstLanelets & lanelet_section1, + const lanelet::ConstLanelets & lanelet_section2) const; + bool getPreviousLaneletWithinRouteExceptGoal( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * prev_lanelet) const; + bool getNextLaneletWithinRouteExceptStart( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * next_lanelet) const; + bool getRightLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const; + bool getLeftLaneletWithinRoute( + const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const; lanelet::ConstLanelets getRouteLanelets() const; lanelet::ConstLanelets getLaneletSequenceUpTo( const lanelet::ConstLanelet & lanelet, diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 0c0d14f95c768..5b8c7ce247208 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -371,10 +371,10 @@ void RouteHandler::setLaneletsFromRouteMsg() } route_lanelets_.clear(); preferred_lanelets_.clear(); - const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_, lanelet_map_ptr_); - if (!is_route_valid) { - return; - } + // const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_, + // lanelet_map_ptr_); if (!is_route_valid) { + // return; + // } size_t primitive_size{0}; for (const auto & route_section : route_ptr_->segments) { @@ -384,11 +384,19 @@ void RouteHandler::setLaneletsFromRouteMsg() for (const auto & route_section : route_ptr_->segments) { for (const auto & primitive : route_section.primitives) { - const auto id = primitive.id; - const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); - route_lanelets_.push_back(llt); - if (id == route_section.preferred_primitive.id) { - preferred_lanelets_.push_back(llt); + try { + const auto id = primitive.id; + const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); + route_lanelets_.push_back(llt); + if (id == route_section.preferred_primitive.id) { + preferred_lanelets_.push_back(llt); + } + } catch (const std::exception & e) { + std::cerr + << e.what() + << ". Maybe the loaded route was created on a different Map from the current one. " + "Try to load the other Route again." + << std::endl; } } } @@ -397,15 +405,31 @@ void RouteHandler::setLaneletsFromRouteMsg() if (!route_ptr_->segments.empty()) { goal_lanelets_.reserve(route_ptr_->segments.back().primitives.size()); for (const auto & primitive : route_ptr_->segments.back().primitives) { - const auto id = primitive.id; - const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); - goal_lanelets_.push_back(llt); + try { + const auto id = primitive.id; + const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); + goal_lanelets_.push_back(llt); + } catch (const std::exception & e) { + std::cerr + << e.what() + << ". Maybe the loaded route was created on a different Map from the current one. " + "Try to load the other Route again." + << std::endl; + } } start_lanelets_.reserve(route_ptr_->segments.front().primitives.size()); for (const auto & primitive : route_ptr_->segments.front().primitives) { - const auto id = primitive.id; - const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); - start_lanelets_.push_back(llt); + try { + const auto id = primitive.id; + const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); + start_lanelets_.push_back(llt); + } catch (const std::exception & e) { + std::cerr + << e.what() + << ". Maybe the loaded route was created on a different Map from the current one. " + "Try to load the other Route again." + << std::endl; + } } } is_handler_ready_ = true; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 135e420d113f3..c21159026b0ce 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -403,6 +403,7 @@ void BehaviorPathPlannerNode::run() // update map if (map_ptr) { planner_data_->route_handler->setMap(*map_ptr); + planner_manager_->resetRootLanelet(planner_data_); } std::unique_lock lk_manager(mutex_manager_); // for planner_manager_ From d26c53daa5338618e1c942b54f34d8e9ed0dac45 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bar=C4=B1=C5=9F=20Zeren?= Date: Fri, 8 Mar 2024 16:19:43 +0300 Subject: [PATCH 02/14] update planner, add map parameter MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Barış Zeren --- .../launch/planning.launch.xml | 3 + .../scenario_planning/lane_driving.launch.xml | 2 + .../behavior_planning.launch.xml | 7 +- .../scenario_planning.launch.xml | 2 + .../autoware/route_handler/route_handler.hpp | 6 +- .../src/route_handler.cpp | 72 ++++++++++++------- .../src/behavior_path_planner_node.cpp | 9 ++- .../src/planner_manager.cpp | 6 +- .../parameters.hpp | 3 + .../utils/utils.hpp | 3 +- .../src/utils/utils.cpp | 3 +- 11 files changed, 81 insertions(+), 35 deletions(-) diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index e71253c8a978b..f4bfc2da96a35 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -9,6 +9,8 @@ + + @@ -26,6 +28,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 1ec74793b741b..04aaf4651f9ee 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -1,5 +1,6 @@ + @@ -12,6 +13,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index d88601497096d..0f3cb9a9943ef 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -1,8 +1,12 @@ + + + - + + @@ -192,6 +196,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 9082ebab013ed..ff117794e321b 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -1,5 +1,6 @@ + @@ -55,6 +56,7 @@ + diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index 5467b48674d5f..ae82692da7fb8 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -72,8 +72,8 @@ class RouteHandler explicit RouteHandler(const LaneletMapBin & map_msg); // non-const methods - void setMap(const LaneletMapBin & map_msg); - void setRoute(const LaneletRoute & route_msg); + void setMap(const HADMapBin & map_msg, const bool & is_enable_differantial_lanelet = false); + void setRoute(const LaneletRoute & route_msg, const bool & is_enable_differantial_lanelet = false); void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets); void clearRoute(); @@ -349,7 +349,7 @@ class RouteHandler Pose original_goal_pose_; // non-const methods - void setLaneletsFromRouteMsg(); + void setLaneletsFromRouteMsg(const bool & is_enable_differantial_lanelet = false); // const methods // for routing diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 5b8c7ce247208..213f5a8e3c344 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -182,7 +182,7 @@ RouteHandler::RouteHandler(const LaneletMapBin & map_msg) route_ptr_ = nullptr; } -void RouteHandler::setMap(const LaneletMapBin & map_msg) +void RouteHandler::setMap(const HADMapBin & map_msg, const bool & is_enable_differantial_lanelet) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( @@ -214,7 +214,7 @@ void RouteHandler::setMap(const LaneletMapBin & map_msg) is_map_msg_ready_ = true; is_handler_ready_ = false; - setLaneletsFromRouteMsg(); + setLaneletsFromRouteMsg(is_enable_differantial_lanelet); } bool RouteHandler::isRouteLooped(const RouteSections & route_sections) @@ -231,7 +231,8 @@ bool RouteHandler::isRouteLooped(const RouteSections & route_sections) return false; } -void RouteHandler::setRoute(const LaneletRoute & route_msg) +void RouteHandler::setRoute( + const LaneletRoute & route_msg, const bool & is_enable_differantial_lanelet) { if (!isRouteLooped(route_msg.segments)) { // if get not modified route but new route, reset original start pose @@ -241,7 +242,7 @@ void RouteHandler::setRoute(const LaneletRoute & route_msg) } route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; - setLaneletsFromRouteMsg(); + setLaneletsFromRouteMsg(is_enable_differantial_lanelet); } else { RCLCPP_ERROR( logger_, @@ -364,17 +365,19 @@ void RouteHandler::clearRoute() is_handler_ready_ = false; } -void RouteHandler::setLaneletsFromRouteMsg() +void RouteHandler::setLaneletsFromRouteMsg(const bool & is_enable_differantial_lanelet) { if (!route_ptr_ || !is_map_msg_ready_) { return; } route_lanelets_.clear(); preferred_lanelets_.clear(); - // const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_, - // lanelet_map_ptr_); if (!is_route_valid) { - // return; - // } + if (!is_enable_differantial_lanelet) { + const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_, lanelet_map_ptr_); + if (!is_route_valid) { + return; + } + } size_t primitive_size{0}; for (const auto & route_section : route_ptr_->segments) { @@ -392,11 +395,18 @@ void RouteHandler::setLaneletsFromRouteMsg() preferred_lanelets_.push_back(llt); } } catch (const std::exception & e) { - std::cerr - << e.what() - << ". Maybe the loaded route was created on a different Map from the current one. " - "Try to load the other Route again." - << std::endl; + if (!is_enable_differantial_lanelet) { + std::cerr + << e.what() + << ". Maybe the loaded route was created on a different Map from the current one. " + "Try to load the other Route again." + << std::endl; + return; + } else { + RCLCPP_DEBUG( + logger_, "Failed to get lanelet differential map with id: %ld. %s", primitive.id, + e.what()); + } } } } @@ -410,11 +420,18 @@ void RouteHandler::setLaneletsFromRouteMsg() const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); goal_lanelets_.push_back(llt); } catch (const std::exception & e) { - std::cerr - << e.what() - << ". Maybe the loaded route was created on a different Map from the current one. " - "Try to load the other Route again." - << std::endl; + if (!is_enable_differantial_lanelet) { + std::cerr + << e.what() + << ". Maybe the loaded route was created on a different Map from the current one. " + "Try to load the other Route again." + << std::endl; + return; + } else { + RCLCPP_DEBUG( + logger_, "Failed to get lanelet differential map with id: %ld. %s", primitive.id, + e.what()); + } } } start_lanelets_.reserve(route_ptr_->segments.front().primitives.size()); @@ -424,11 +441,18 @@ void RouteHandler::setLaneletsFromRouteMsg() const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); start_lanelets_.push_back(llt); } catch (const std::exception & e) { - std::cerr - << e.what() - << ". Maybe the loaded route was created on a different Map from the current one. " - "Try to load the other Route again." - << std::endl; + if (!is_enable_differantial_lanelet) { + std::cerr + << e.what() + << ". Maybe the loaded route was created on a different Map from the current one. " + "Try to load the other Route again." + << std::endl; + return; + } else { + RCLCPP_DEBUG( + logger_, "Failed to get lanelet differential map with id: %ld. %s", primitive.id, + e.what()); + } } } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index c21159026b0ce..c0e0c712503c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -219,6 +219,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + p.enable_differential_map_loading = declare_parameter("enable_differential_map_loading"); + return p; } @@ -402,7 +404,8 @@ void BehaviorPathPlannerNode::run() // update map if (map_ptr) { - planner_data_->route_handler->setMap(*map_ptr); + planner_data_->route_handler->setMap( + *map_ptr, planner_data_->parameters.enable_differential_map_loading); planner_manager_->resetRootLanelet(planner_data_); } @@ -411,7 +414,9 @@ void BehaviorPathPlannerNode::run() // update route const bool is_first_time = !(planner_data_->route_handler->isHandlerReady()); if (route_ptr) { - planner_data_->route_handler->setRoute(*route_ptr); + planner_data_->route_handler->setRoute( + *route_ptr, planner_data_->parameters.enable_differential_map_loading); + planner_manager_->resetRootLanelet(planner_data_); // uuid is not changed when rerouting with modified goal, // in this case do not need to reset modules. const bool has_same_route_id = diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index dc6f7552c5a61..30a51ecbe3a80 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -137,9 +137,9 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da updateCurrentRouteLanelet(data); - const bool is_out_of_route = utils::isEgoOutOfRoute( - data->self_odometry->pose.pose, current_route_lanelet_->value(), data->prev_modified_goal, - data->route_handler); + const bool is_out_of_route = utils::isEgoOutOfRoute( + data->self_odometry->pose.pose, current_route_lanelet_.value(), data->prev_modified_goal, + data->route_handler, data->parameters); if (!is_any_module_running && is_out_of_route) { BehaviorModuleOutput result_output = utils::createGoalAroundPath(data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp index ecbe9b5208347..d41541bcbdce8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/parameters.hpp @@ -73,6 +73,9 @@ struct BehaviorPathPlannerParameters double right_over_hang; double base_link2front; double base_link2rear; + + // enable/disable differential map loading + bool enable_differential_map_loading; }; #endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 03e6c2d7f8167..820fa5ca20730 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -237,7 +237,8 @@ bool isInLaneletWithYawThreshold( bool isEgoOutOfRoute( const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane, const std::optional & modified_goal, - const std::shared_ptr & route_handler); + const std::shared_ptr & route_handler, + const BehaviorPathPlannerParameters & common_param); bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index fd55d38518cda..37972b4bb0b82 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -493,7 +493,8 @@ bool isInLaneletWithYawThreshold( bool isEgoOutOfRoute( const Pose & self_pose, const lanelet::ConstLanelet & closest_road_lane, const std::optional & modified_goal, - const std::shared_ptr & route_handler) + const std::shared_ptr & route_handler, + const BehaviorPathPlannerParameters & common_param) { const Pose & goal_pose = (modified_goal && modified_goal->uuid == route_handler->getRouteUuid()) ? modified_goal->pose From bfb00796ab59405ac9280ff72c53318067295650 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bar=C4=B1=C5=9F=20Zeren?= Date: Fri, 8 Mar 2024 16:21:57 +0300 Subject: [PATCH 03/14] update params MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Barış Zeren --- launch/tier4_planning_launch/launch/planning.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index f4bfc2da96a35..0cca0348b05cd 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -10,7 +10,7 @@ - + From 21ede49a361911565d82a99a58090d3e275a9bce Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 12 Mar 2024 13:19:14 +0000 Subject: [PATCH 04/14] style(pre-commit): autofix --- .../include/autoware/route_handler/route_handler.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index ae82692da7fb8..1aecb9150a9f1 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -73,7 +73,8 @@ class RouteHandler // non-const methods void setMap(const HADMapBin & map_msg, const bool & is_enable_differantial_lanelet = false); - void setRoute(const LaneletRoute & route_msg, const bool & is_enable_differantial_lanelet = false); + void setRoute( + const LaneletRoute & route_msg, const bool & is_enable_differantial_lanelet = false); void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets); void clearRoute(); From 9e91ac539725cafef731f95eb94cdc613952832e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bar=C4=B1=C5=9F=20Zeren?= Date: Wed, 13 Mar 2024 12:05:57 +0300 Subject: [PATCH 05/14] update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Barış Zeren --- .../src/behavior_path_planner_node.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index c0e0c712503c6..6ded246fe3567 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -406,7 +406,9 @@ void BehaviorPathPlannerNode::run() if (map_ptr) { planner_data_->route_handler->setMap( *map_ptr, planner_data_->parameters.enable_differential_map_loading); - planner_manager_->resetRootLanelet(planner_data_); + if (planner_data_->parameters.enable_differential_map_loading) { + planner_manager_->resetRootLanelet(planner_data_); + } } std::unique_lock lk_manager(mutex_manager_); // for planner_manager_ From 195a8c79c437c06432cdd617b64e3a5792495081 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bar=C4=B1=C5=9F=20Zeren?= Date: Wed, 27 Mar 2024 14:12:32 +0300 Subject: [PATCH 06/14] Fix spell-check error MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Barış Zeren --- .../autoware/route_handler/route_handler.hpp | 6 +++--- .../src/route_handler.cpp | 18 +++++++++--------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index 1aecb9150a9f1..c0ecaba2dc56d 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -72,9 +72,9 @@ class RouteHandler explicit RouteHandler(const LaneletMapBin & map_msg); // non-const methods - void setMap(const HADMapBin & map_msg, const bool & is_enable_differantial_lanelet = false); + void setMap(const HADMapBin & map_msg, const bool & is_enable_differential_lanelet = false); void setRoute( - const LaneletRoute & route_msg, const bool & is_enable_differantial_lanelet = false); + const LaneletRoute & route_msg, const bool & is_enable_differential_lanelet = false); void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets); void clearRoute(); @@ -350,7 +350,7 @@ class RouteHandler Pose original_goal_pose_; // non-const methods - void setLaneletsFromRouteMsg(const bool & is_enable_differantial_lanelet = false); + void setLaneletsFromRouteMsg(const bool & is_enable_differential_lanelet = false); // const methods // for routing diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 213f5a8e3c344..9c2dde6f8e2ad 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -182,7 +182,7 @@ RouteHandler::RouteHandler(const LaneletMapBin & map_msg) route_ptr_ = nullptr; } -void RouteHandler::setMap(const HADMapBin & map_msg, const bool & is_enable_differantial_lanelet) +void RouteHandler::setMap(const HADMapBin & map_msg, const bool & is_enable_differential_lanelet) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( @@ -214,7 +214,7 @@ void RouteHandler::setMap(const HADMapBin & map_msg, const bool & is_enable_diff is_map_msg_ready_ = true; is_handler_ready_ = false; - setLaneletsFromRouteMsg(is_enable_differantial_lanelet); + setLaneletsFromRouteMsg(is_enable_differential_lanelet); } bool RouteHandler::isRouteLooped(const RouteSections & route_sections) @@ -232,7 +232,7 @@ bool RouteHandler::isRouteLooped(const RouteSections & route_sections) } void RouteHandler::setRoute( - const LaneletRoute & route_msg, const bool & is_enable_differantial_lanelet) + const LaneletRoute & route_msg, const bool & is_enable_differential_lanelet) { if (!isRouteLooped(route_msg.segments)) { // if get not modified route but new route, reset original start pose @@ -242,7 +242,7 @@ void RouteHandler::setRoute( } route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; - setLaneletsFromRouteMsg(is_enable_differantial_lanelet); + setLaneletsFromRouteMsg(is_enable_differential_lanelet); } else { RCLCPP_ERROR( logger_, @@ -365,14 +365,14 @@ void RouteHandler::clearRoute() is_handler_ready_ = false; } -void RouteHandler::setLaneletsFromRouteMsg(const bool & is_enable_differantial_lanelet) +void RouteHandler::setLaneletsFromRouteMsg(const bool & is_enable_differential_lanelet) { if (!route_ptr_ || !is_map_msg_ready_) { return; } route_lanelets_.clear(); preferred_lanelets_.clear(); - if (!is_enable_differantial_lanelet) { + if (!is_enable_differential_lanelet) { const bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr_, lanelet_map_ptr_); if (!is_route_valid) { return; @@ -395,7 +395,7 @@ void RouteHandler::setLaneletsFromRouteMsg(const bool & is_enable_differantial_l preferred_lanelets_.push_back(llt); } } catch (const std::exception & e) { - if (!is_enable_differantial_lanelet) { + if (!is_enable_differential_lanelet) { std::cerr << e.what() << ". Maybe the loaded route was created on a different Map from the current one. " @@ -420,7 +420,7 @@ void RouteHandler::setLaneletsFromRouteMsg(const bool & is_enable_differantial_l const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); goal_lanelets_.push_back(llt); } catch (const std::exception & e) { - if (!is_enable_differantial_lanelet) { + if (!is_enable_differential_lanelet) { std::cerr << e.what() << ". Maybe the loaded route was created on a different Map from the current one. " @@ -441,7 +441,7 @@ void RouteHandler::setLaneletsFromRouteMsg(const bool & is_enable_differantial_l const auto & llt = lanelet_map_ptr_->laneletLayer.get(id); start_lanelets_.push_back(llt); } catch (const std::exception & e) { - if (!is_enable_differantial_lanelet) { + if (!is_enable_differential_lanelet) { std::cerr << e.what() << ". Maybe the loaded route was created on a different Map from the current one. " From 8c67ec70dc2a1320a9800c8b0ffe6ab08193a565 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bar=C4=B1=C5=9F=20Zeren?= Date: Thu, 6 Jun 2024 11:55:54 +0300 Subject: [PATCH 07/14] chore: remove conflicts MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Barış Zeren --- .../autoware/route_handler/route_handler.hpp | 2 +- .../src/route_handler.cpp | 2 +- .../src/behavior_path_planner_node.cpp | 4 --- .../src/utils/utils.cpp | 32 +++++++++++++++++-- 4 files changed, 32 insertions(+), 8 deletions(-) diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index c0ecaba2dc56d..ac992bccdd3be 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -72,7 +72,7 @@ class RouteHandler explicit RouteHandler(const LaneletMapBin & map_msg); // non-const methods - void setMap(const HADMapBin & map_msg, const bool & is_enable_differential_lanelet = false); + void setMap(const LaneletMapBin & map_msg, const bool & is_enable_differential_lanelet = false); void setRoute( const LaneletRoute & route_msg, const bool & is_enable_differential_lanelet = false); void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets); diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 9c2dde6f8e2ad..3bd96c294ce56 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -182,7 +182,7 @@ RouteHandler::RouteHandler(const LaneletMapBin & map_msg) route_ptr_ = nullptr; } -void RouteHandler::setMap(const HADMapBin & map_msg, const bool & is_enable_differential_lanelet) +void RouteHandler::setMap(const LaneletMapBin & map_msg, const bool & is_enable_differential_lanelet) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 6ded246fe3567..11b42a1da33cd 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -406,9 +406,6 @@ void BehaviorPathPlannerNode::run() if (map_ptr) { planner_data_->route_handler->setMap( *map_ptr, planner_data_->parameters.enable_differential_map_loading); - if (planner_data_->parameters.enable_differential_map_loading) { - planner_manager_->resetRootLanelet(planner_data_); - } } std::unique_lock lk_manager(mutex_manager_); // for planner_manager_ @@ -418,7 +415,6 @@ void BehaviorPathPlannerNode::run() if (route_ptr) { planner_data_->route_handler->setRoute( *route_ptr, planner_data_->parameters.enable_differential_map_loading); - planner_manager_->resetRootLanelet(planner_data_); // uuid is not changed when rerouting with modified goal, // in this case do not need to reset modules. const bool has_same_route_id = diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 37972b4bb0b82..8c324757f06f3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -503,8 +503,36 @@ bool isEgoOutOfRoute( lanelet::ConstLanelet goal_lane; const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose); if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front(); - const auto is_failed_getting_lanelet = - shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane); + + const bool is_failed_getting_lanelet = std::invoke([&]() { + if (utils::isInLanelets(goal_pose, shoulder_goal_lanes)) { + return !lanelet::utils::query::getClosestLanelet(shoulder_goal_lanes, goal_pose, &goal_lane); + } + if (common_param.enable_differential_map_loading) { + // If dynamic map is enabled, get the goal lanelet from the current lanelet map + // + // Check the route lanelets and last route element in the current lanelet map + // will be the goal lanelet + + lanelet::LaneletMapPtr map = route_handler->getLaneletMapPtr(); + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map); + lanelet::ConstLanelets route_lanelets = route_handler->getRouteLanelets(); + + auto goalLaneletIt = + std::find_if(all_lanelets.rbegin(), all_lanelets.rend(), [&](const auto & currentLanelet) { + return currentLanelet.id() == route_lanelets.back().id(); + }); + + if (goalLaneletIt != all_lanelets.rend()) { + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("util"), + "Found goal lanelet in current lanelet map"); + goal_lane = *goalLaneletIt; + return false; + } + } + return !route_handler->getGoalLanelet(&goal_lane); + }); if (is_failed_getting_lanelet) { RCLCPP_WARN_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet"); From 57db08fbab41b2dfd3717092e6a2d6e4e39de7a1 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 6 Jun 2024 08:58:10 +0000 Subject: [PATCH 08/14] style(pre-commit): autofix --- .../src/route_handler.cpp | 3 +- .../src/utils/utils.cpp | 58 +++++++++---------- 2 files changed, 31 insertions(+), 30 deletions(-) diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 3bd96c294ce56..e757444225335 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -182,7 +182,8 @@ RouteHandler::RouteHandler(const LaneletMapBin & map_msg) route_ptr_ = nullptr; } -void RouteHandler::setMap(const LaneletMapBin & map_msg, const bool & is_enable_differential_lanelet) +void RouteHandler::setMap( + const LaneletMapBin & map_msg, const bool & is_enable_differential_lanelet) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 8c324757f06f3..8e81979f77037 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -504,35 +504,35 @@ bool isEgoOutOfRoute( const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose); if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front(); - const bool is_failed_getting_lanelet = std::invoke([&]() { - if (utils::isInLanelets(goal_pose, shoulder_goal_lanes)) { - return !lanelet::utils::query::getClosestLanelet(shoulder_goal_lanes, goal_pose, &goal_lane); - } - if (common_param.enable_differential_map_loading) { - // If dynamic map is enabled, get the goal lanelet from the current lanelet map - // - // Check the route lanelets and last route element in the current lanelet map - // will be the goal lanelet - - lanelet::LaneletMapPtr map = route_handler->getLaneletMapPtr(); - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map); - lanelet::ConstLanelets route_lanelets = route_handler->getRouteLanelets(); - - auto goalLaneletIt = - std::find_if(all_lanelets.rbegin(), all_lanelets.rend(), [&](const auto & currentLanelet) { - return currentLanelet.id() == route_lanelets.back().id(); - }); - - if (goalLaneletIt != all_lanelets.rend()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util"), - "Found goal lanelet in current lanelet map"); - goal_lane = *goalLaneletIt; - return false; - } - } - return !route_handler->getGoalLanelet(&goal_lane); - }); + const bool is_failed_getting_lanelet = std::invoke([&]() { + if (utils::isInLanelets(goal_pose, shoulder_goal_lanes)) { + return !lanelet::utils::query::getClosestLanelet(shoulder_goal_lanes, goal_pose, &goal_lane); + } + if (common_param.enable_differential_map_loading) { + // If dynamic map is enabled, get the goal lanelet from the current lanelet map + // + // Check the route lanelets and last route element in the current lanelet map + // will be the goal lanelet + + lanelet::LaneletMapPtr map = route_handler->getLaneletMapPtr(); + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map); + lanelet::ConstLanelets route_lanelets = route_handler->getRouteLanelets(); + + auto goalLaneletIt = + std::find_if(all_lanelets.rbegin(), all_lanelets.rend(), [&](const auto & currentLanelet) { + return currentLanelet.id() == route_lanelets.back().id(); + }); + + if (goalLaneletIt != all_lanelets.rend()) { + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("util"), + "Found goal lanelet in current lanelet map"); + goal_lane = *goalLaneletIt; + return false; + } + } + return !route_handler->getGoalLanelet(&goal_lane); + }); if (is_failed_getting_lanelet) { RCLCPP_WARN_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet"); From 6fbdc6b93c2a9df91721482e39caeead6f0fc932 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bar=C4=B1=C5=9F=20Zeren?= Date: Thu, 6 Jun 2024 15:42:07 +0300 Subject: [PATCH 09/14] update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Barış Zeren --- .../src/utils/utils.cpp | 51 +++++++++---------- 1 file changed, 25 insertions(+), 26 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 8e81979f77037..40ea9c765515c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -504,35 +504,34 @@ bool isEgoOutOfRoute( const auto shoulder_goal_lanes = route_handler->getShoulderLaneletsAtPose(goal_pose); if (!shoulder_goal_lanes.empty()) goal_lane = shoulder_goal_lanes.front(); - const bool is_failed_getting_lanelet = std::invoke([&]() { - if (utils::isInLanelets(goal_pose, shoulder_goal_lanes)) { - return !lanelet::utils::query::getClosestLanelet(shoulder_goal_lanes, goal_pose, &goal_lane); - } - if (common_param.enable_differential_map_loading) { - // If dynamic map is enabled, get the goal lanelet from the current lanelet map - // - // Check the route lanelets and last route element in the current lanelet map - // will be the goal lanelet - - lanelet::LaneletMapPtr map = route_handler->getLaneletMapPtr(); - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map); - lanelet::ConstLanelets route_lanelets = route_handler->getRouteLanelets(); - - auto goalLaneletIt = - std::find_if(all_lanelets.rbegin(), all_lanelets.rend(), [&](const auto & currentLanelet) { - return currentLanelet.id() == route_lanelets.back().id(); - }); - - if (goalLaneletIt != all_lanelets.rend()) { + bool is_failed_getting_lanelet; + if (!common_param.enable_differential_map_loading) { + is_failed_getting_lanelet = shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane); + } else { + // If dynamic map is enabled, get the goal lanelet from the current lanelet map + // + // Check the route lanelets and last route element in the current lanelet map + // will be the goal lanelet + + lanelet::LaneletMapPtr map = route_handler->getLaneletMapPtr(); + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(map); + lanelet::ConstLanelets route_lanelets = route_handler->getRouteLanelets(); + + auto goalLaneletIt = + std::find_if(all_lanelets.rbegin(), all_lanelets.rend(), [&](const auto & currentLanelet) { + return currentLanelet.id() == route_lanelets.back().id(); + }); + + if (goalLaneletIt != all_lanelets.rend()) { RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util"), - "Found goal lanelet in current lanelet map"); + rclcpp::get_logger("behavior_path_planner").get_child("util"), + "Found goal lanelet in current lanelet map"); goal_lane = *goalLaneletIt; - return false; - } + is_failed_getting_lanelet = false; } - return !route_handler->getGoalLanelet(&goal_lane); - }); + is_failed_getting_lanelet = !route_handler->getGoalLanelet(&goal_lane); + } + if (is_failed_getting_lanelet) { RCLCPP_WARN_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet"); From 796f3d8b57199bdf83efdbb080160e1c307750af Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 6 Jun 2024 12:44:23 +0000 Subject: [PATCH 10/14] style(pre-commit): autofix --- .../src/utils/utils.cpp | 21 ++++++++++--------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 40ea9c765515c..3a39ceec577f1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -506,7 +506,8 @@ bool isEgoOutOfRoute( bool is_failed_getting_lanelet; if (!common_param.enable_differential_map_loading) { - is_failed_getting_lanelet = shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane); + is_failed_getting_lanelet = + shoulder_goal_lanes.empty() && !route_handler->getGoalLanelet(&goal_lane); } else { // If dynamic map is enabled, get the goal lanelet from the current lanelet map // @@ -518,18 +519,18 @@ bool isEgoOutOfRoute( lanelet::ConstLanelets route_lanelets = route_handler->getRouteLanelets(); auto goalLaneletIt = - std::find_if(all_lanelets.rbegin(), all_lanelets.rend(), [&](const auto & currentLanelet) { - return currentLanelet.id() == route_lanelets.back().id(); - }); + std::find_if(all_lanelets.rbegin(), all_lanelets.rend(), [&](const auto & currentLanelet) { + return currentLanelet.id() == route_lanelets.back().id(); + }); if (goalLaneletIt != all_lanelets.rend()) { - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner").get_child("util"), - "Found goal lanelet in current lanelet map"); - goal_lane = *goalLaneletIt; - is_failed_getting_lanelet = false; + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("util"), + "Found goal lanelet in current lanelet map"); + goal_lane = *goalLaneletIt; + is_failed_getting_lanelet = false; } - is_failed_getting_lanelet = !route_handler->getGoalLanelet(&goal_lane); + is_failed_getting_lanelet = !route_handler->getGoalLanelet(&goal_lane); } if (is_failed_getting_lanelet) { From 97e327cfca6cda8bdcc931475033dab5d525bf01 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bar=C4=B1=C5=9F=20Zeren?= Date: Thu, 6 Jun 2024 16:13:03 +0300 Subject: [PATCH 11/14] update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Barış Zeren --- launch/tier4_planning_launch/launch/planning.launch.xml | 2 +- .../autoware_behavior_path_planner_common/src/utils/utils.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 0cca0348b05cd..2c1fe49c6ef0a 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -10,7 +10,7 @@ - + diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index 3a39ceec577f1..4946061c7d41b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -530,7 +530,6 @@ bool isEgoOutOfRoute( goal_lane = *goalLaneletIt; is_failed_getting_lanelet = false; } - is_failed_getting_lanelet = !route_handler->getGoalLanelet(&goal_lane); } if (is_failed_getting_lanelet) { From eee5d51a41ffe117ccb54a39ddb583fc84cf2d67 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bar=C4=B1=C5=9F=20Zeren?= Date: Sun, 25 Aug 2024 17:37:06 +0300 Subject: [PATCH 12/14] chore: update MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Barış Zeren --- .../include/autoware/route_handler/route_handler.hpp | 1 - .../autoware_behavior_path_planner/src/planner_manager.cpp | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp index ac992bccdd3be..f292e547aaf3a 100644 --- a/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware/route_handler/route_handler.hpp @@ -368,7 +368,6 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * right_lanelet) const; bool getLeftLaneletWithinRoute( const lanelet::ConstLanelet & lanelet, lanelet::ConstLanelet * left_lanelet) const; - lanelet::ConstLanelets getRouteLanelets() const; lanelet::ConstLanelets getLaneletSequenceUpTo( const lanelet::ConstLanelet & lanelet, const double min_length = std::numeric_limits::max(), diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 30a51ecbe3a80..d8293d36e61f3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -138,7 +138,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da updateCurrentRouteLanelet(data); const bool is_out_of_route = utils::isEgoOutOfRoute( - data->self_odometry->pose.pose, current_route_lanelet_.value(), data->prev_modified_goal, + data->self_odometry->pose.pose, current_route_lanelet_->value(), data->prev_modified_goal, data->route_handler, data->parameters); if (!is_any_module_running && is_out_of_route) { From f6c00cb44c520a5cb5214000bdd6264ab324d311 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sun, 25 Aug 2024 14:39:23 +0000 Subject: [PATCH 13/14] style(pre-commit): autofix --- .../autoware_behavior_path_planner/src/planner_manager.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index d8293d36e61f3..92baf0c7cac81 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -137,9 +137,9 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da updateCurrentRouteLanelet(data); - const bool is_out_of_route = utils::isEgoOutOfRoute( - data->self_odometry->pose.pose, current_route_lanelet_->value(), data->prev_modified_goal, - data->route_handler, data->parameters); + const bool is_out_of_route = utils::isEgoOutOfRoute( + data->self_odometry->pose.pose, current_route_lanelet_->value(), data->prev_modified_goal, + data->route_handler, data->parameters); if (!is_any_module_running && is_out_of_route) { BehaviorModuleOutput result_output = utils::createGoalAroundPath(data); From 0833903dfec84df59158627cf6335472fd9ee8aa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bar=C4=B1=C5=9F=20Zeren?= Date: Mon, 26 Aug 2024 14:15:55 +0300 Subject: [PATCH 14/14] fix: planner crash MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Barış Zeren --- .../src/behavior_path_planner_node.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 11b42a1da33cd..4c3ee137d747a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -406,6 +406,9 @@ void BehaviorPathPlannerNode::run() if (map_ptr) { planner_data_->route_handler->setMap( *map_ptr, planner_data_->parameters.enable_differential_map_loading); + if (planner_data_->parameters.enable_differential_map_loading) { + planner_manager_->resetCurrentRouteLanelet(planner_data_); + } } std::unique_lock lk_manager(mutex_manager_); // for planner_manager_