diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index e71253c8a978b..2c1fe49c6ef0a 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 13e13b642af1f..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 @@ -72,8 +72,9 @@ 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 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); void clearRoute(); @@ -325,6 +326,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 @@ -348,14 +350,24 @@ class RouteHandler Pose original_goal_pose_; // non-const methods - void setLaneletsFromRouteMsg(); + void setLaneletsFromRouteMsg(const bool & is_enable_differential_lanelet = false); // const methods // for routing lanelet::ConstLanelets getMainLanelets(const lanelet::ConstLanelets & path_lanelets) const; // for lanelet - lanelet::ConstLanelets getRouteLanelets() const; + 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 getLaneletSequenceUpTo( const lanelet::ConstLanelet & lanelet, const double min_length = std::numeric_limits::max(), diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 0c0d14f95c768..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) +void RouteHandler::setMap( + const LaneletMapBin & map_msg, const bool & is_enable_differential_lanelet) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( @@ -214,7 +215,7 @@ void RouteHandler::setMap(const LaneletMapBin & map_msg) is_map_msg_ready_ = true; is_handler_ready_ = false; - setLaneletsFromRouteMsg(); + setLaneletsFromRouteMsg(is_enable_differential_lanelet); } bool RouteHandler::isRouteLooped(const RouteSections & route_sections) @@ -231,7 +232,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_differential_lanelet) { if (!isRouteLooped(route_msg.segments)) { // if get not modified route but new route, reset original start pose @@ -241,7 +243,7 @@ void RouteHandler::setRoute(const LaneletRoute & route_msg) } route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; - setLaneletsFromRouteMsg(); + setLaneletsFromRouteMsg(is_enable_differential_lanelet); } else { RCLCPP_ERROR( logger_, @@ -364,16 +366,18 @@ void RouteHandler::clearRoute() is_handler_ready_ = false; } -void RouteHandler::setLaneletsFromRouteMsg() +void RouteHandler::setLaneletsFromRouteMsg(const bool & is_enable_differential_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_differential_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}; @@ -384,11 +388,26 @@ 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) { + if (!is_enable_differential_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()); + } } } } @@ -397,15 +416,45 @@ 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) { + if (!is_enable_differential_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()); 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) { + if (!is_enable_differential_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()); + } + } } } 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..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 @@ -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,11 @@ 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); + if (planner_data_->parameters.enable_differential_map_loading) { + planner_manager_->resetCurrentRouteLanelet(planner_data_); + } } std::unique_lock lk_manager(mutex_manager_); // for planner_manager_ @@ -410,7 +416,8 @@ 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); // 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..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 @@ -139,7 +139,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da 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->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..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 @@ -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 @@ -502,8 +503,35 @@ 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); + + 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"); + goal_lane = *goalLaneletIt; + is_failed_getting_lanelet = false; + } + } + if (is_failed_getting_lanelet) { RCLCPP_WARN_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("util"), "cannot find goal lanelet");