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");