feat(behavior_velocity,behavior_path,route_handler): use route handler in behavior velocity

planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp

@@ -338,8 +338,9 @@ std::pair<bool, bool> LaneChangeModule::getSafePath(
 
     // select valid path
     valid_paths = lane_change_utils::selectValidPaths(
-      lane_change_paths, current_lanes, check_lanes, route_handler->getOverallGraph(), current_pose,
-      route_handler->isInGoalRouteSection(current_lanes.back()), route_handler->getGoalPose());
+      lane_change_paths, current_lanes, check_lanes, *route_handler->getOverallGraphPtr(),
+      current_pose, route_handler->isInGoalRouteSection(current_lanes.back()),
+      route_handler->getGoalPose());
 
     if (valid_paths.empty()) {
       return std::make_pair(false, false);

planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp

@@ -422,7 +422,7 @@ std::pair<bool, bool> PullOutModule::getSafePath(
 
     // select valid path
     valid_paths = pull_out_utils::selectValidPaths(
-      pull_out_paths, road_lanes, check_lanes, route_handler->getOverallGraph(), current_pose,
+      pull_out_paths, road_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose,
       route_handler->isInGoalRouteSection(road_lanes.back()), route_handler->getGoalPose());
 
     if (valid_paths.empty()) {
@@ -493,7 +493,7 @@ std::pair<bool, bool> PullOutModule::getSafeRetreatPath(
 
     // select valid path
     valid_paths = pull_out_utils::selectValidPaths(
-      pull_out_paths, road_lanes, check_lanes, route_handler->getOverallGraph(), current_pose,
+      pull_out_paths, road_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose,
       route_handler->isInGoalRouteSection(road_lanes.back()), route_handler->getGoalPose());
 
     if (valid_paths.empty()) {
@@ -574,7 +574,7 @@ bool PullOutModule::getBackDistance(
 
       // select valid path
       valid_paths = pull_out_utils::selectValidPaths(
-        pull_out_paths, road_lanes, check_lanes, route_handler->getOverallGraph(), current_pose,
+        pull_out_paths, road_lanes, check_lanes, *route_handler->getOverallGraphPtr(), current_pose,
         route_handler->isInGoalRouteSection(road_lanes.back()), route_handler->getGoalPose());
 
       if (valid_paths.empty()) {

planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp

@@ -353,8 +353,9 @@ std::pair<bool, bool> PullOverModule::getSafePath(
 
     // select valid path
     valid_paths = pull_over_utils::selectValidPaths(
-      pull_over_paths, current_lanes, check_lanes, route_handler->getOverallGraph(), current_pose,
-      route_handler->isInGoalRouteSection(current_lanes.back()), route_handler->getGoalPose());
+      pull_over_paths, current_lanes, check_lanes, *route_handler->getOverallGraphPtr(),
+      current_pose, route_handler->isInGoalRouteSection(current_lanes.back()),
+      route_handler->getGoalPose());
 
     if (valid_paths.empty()) {
       return std::make_pair(false, false);

planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp

@@ -15,6 +15,8 @@
 #ifndef BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_
 #define BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_
 
+#include "route_handler/route_handler.hpp"
+
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
@@ -69,23 +71,20 @@ struct PlannerData
   std::deque<geometry_msgs::msg::TwistStamped> velocity_buffer;
   autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects;
   pcl::PointCloud<pcl::PointXYZ>::ConstPtr no_ground_pointcloud;
-  lanelet::LaneletMapPtr lanelet_map;
   // occupancy grid
   nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid;
 
   // other internal data
   std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped> traffic_light_id_map;
-  lanelet::traffic_rules::TrafficRulesPtr traffic_rules;
-  lanelet::routing::RoutingGraphPtr routing_graph;
-  std::shared_ptr<const lanelet::routing::RoutingGraphContainer> overall_graphs;
-
   // external data
   std::map<int, autoware_auto_perception_msgs::msg::TrafficSignalStamped>
     external_traffic_light_id_map;
   boost::optional<tier4_api_msgs::msg::CrosswalkStatus> external_crosswalk_status_input;
   boost::optional<tier4_api_msgs::msg::IntersectionStatus> external_intersection_status_input;
   tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states;
 
+  // route handler
+  std::shared_ptr<route_handler::RouteHandler> route_handler_;
   // parameters
   vehicle_info_util::VehicleInfo vehicle_info_;
 

planning/behavior_velocity_planner/package.xml

@@ -27,6 +27,7 @@
   <depend>pcl_conversions</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>route_handler</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>

planning/behavior_velocity_planner/src/node.cpp

@@ -180,7 +180,10 @@ bool BehaviorVelocityPlannerNode::isDataReady()
   if (!d.no_ground_pointcloud) {
     return false;
   }
-  if (!d.lanelet_map) {
+  if (!d.route_handler_) {
+    return false;
+  }
+  if (!d.route_handler_->isMapMsgReady()) {
     return false;
   }
 
@@ -252,32 +255,7 @@ void BehaviorVelocityPlannerNode::onLaneletMap(
   const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg)
 {
   // Load map
-  planner_data_.lanelet_map = std::make_shared<lanelet::LaneletMap>();
-  lanelet::utils::conversion::fromBinMsg(
-    *msg, planner_data_.lanelet_map, &planner_data_.traffic_rules, &planner_data_.routing_graph);
-
-  // Build graph
-  {
-    using lanelet::Locations;
-    using lanelet::Participants;
-    using lanelet::routing::RoutingGraph;
-    using lanelet::routing::RoutingGraphConstPtr;
-    using lanelet::routing::RoutingGraphContainer;
-    using lanelet::traffic_rules::TrafficRulesFactory;
-
-    const auto traffic_rules =
-      TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle);
-    const auto pedestrian_rules =
-      TrafficRulesFactory::create(Locations::Germany, Participants::Pedestrian);
-
-    RoutingGraphConstPtr vehicle_graph =
-      RoutingGraph::build(*planner_data_.lanelet_map, *traffic_rules);
-    RoutingGraphConstPtr pedestrian_graph =
-      RoutingGraph::build(*planner_data_.lanelet_map, *pedestrian_rules);
-    RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph});
-
-    planner_data_.overall_graphs = std::make_shared<const RoutingGraphContainer>(overall_graphs);
-  }
+  planner_data_.route_handler_ = std::make_shared<route_handler::RouteHandler>(*msg);
 }
 
 void BehaviorVelocityPlannerNode::onTrafficSignals(

planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp

@@ -70,7 +70,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node)
 void BlindSpotModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  for (const auto & ll : getLaneletsOnPath(path, planner_data_->lanelet_map)) {
+  for (const auto & ll :
+       getLaneletsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
     const auto lane_id = ll.id();
     const auto module_id = lane_id;
 

planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp

@@ -45,7 +45,8 @@ BlindSpotModule::BlindSpotModule(
   turn_direction_(TurnDirection::INVALID)
 {
   planner_param_ = planner_param;
-  const auto & assigned_lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id);
+  const auto & assigned_lanelet =
+    planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id);
   const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else");
   if (!turn_direction.compare("left")) {
     turn_direction_ = TurnDirection::LEFT;
@@ -74,8 +75,8 @@ bool BlindSpotModule::modifyPathVelocity(
   geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose;
 
   /* get lanelet map */
-  const auto lanelet_map_ptr = planner_data_->lanelet_map;
-  const auto routing_graph_ptr = planner_data_->routing_graph;
+  const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
+  const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
 
   /* set stop-line and stop-judgement-line for base_link */
   int stop_line_idx = -1;
@@ -518,7 +519,8 @@ bool BlindSpotModule::isTargetObjectType(
 boost::optional<geometry_msgs::msg::Point> BlindSpotModule::getStartPointFromLaneLet(
   const int lane_id) const
 {
-  lanelet::ConstLanelet lanelet = planner_data_->lanelet_map->laneletLayer.get(lane_id);
+  lanelet::ConstLanelet lanelet =
+    planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id);
   if (lanelet.centerline().empty()) {
     return boost::none;
   }

planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp

@@ -90,8 +90,9 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
 void CrosswalkModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
+  const auto rh = planner_data_->route_handler_;
   for (const auto & crosswalk :
-       getCrosswalksOnPath(path, planner_data_->lanelet_map, planner_data_->overall_graphs)) {
+       getCrosswalksOnPath(path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr())) {
     const auto module_id = crosswalk.id();
     if (!isModuleRegistered(module_id)) {
       registerModule(std::make_shared<CrosswalkModule>(
@@ -112,8 +113,9 @@ std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
 CrosswalkModuleManager::getModuleExpiredFunction(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
+  const auto rh = planner_data_->route_handler_;
   const auto crosswalk_id_set =
-    getCrosswalkIdSetOnPath(path, planner_data_->lanelet_map, planner_data_->overall_graphs);
+    getCrosswalkIdSetOnPath(path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr());
 
   return [crosswalk_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
     return crosswalk_id_set.count(scene_module->getModuleId()) == 0;

planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp

@@ -175,7 +175,8 @@ lanelet::Optional<lanelet::ConstLineString3d> getStopLineFromMap(
   const int lane_id, const std::shared_ptr<const PlannerData> & planner_data,
   const std::string & attribute_name)
 {
-  lanelet::ConstLanelet lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id);
+  lanelet::ConstLanelet lanelet =
+    planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id);
   const auto road_markings = lanelet.regulatoryElementsAs<lanelet::autoware::RoadMarking>();
   lanelet::ConstLineStrings3d stop_line;
   for (const auto & road_marking : road_markings) {

planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp

@@ -73,7 +73,7 @@ void DetectionAreaModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
   for (const auto & detection_area :
-       getDetectionAreaRegElemsOnPath(path, planner_data_->lanelet_map)) {
+       getDetectionAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
     // Use lanelet_id to unregister module when the route is changed
     const auto module_id = detection_area->id();
     if (!isModuleRegistered(module_id)) {
@@ -88,7 +88,8 @@ std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
 DetectionAreaModuleManager::getModuleExpiredFunction(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  const auto detection_area_id_set = getDetectionAreaIdSetOnPath(path, planner_data_->lanelet_map);
+  const auto detection_area_id_set =
+    getDetectionAreaIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr());
 
   return [detection_area_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
     return detection_area_id_set.count(scene_module->getModuleId()) == 0;

planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp

@@ -100,7 +100,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node
 void IntersectionModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  const auto lanelets = getLaneletsOnPath(path, planner_data_->lanelet_map);
+  const auto lanelets = getLaneletsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr());
   for (size_t i = 0; i < lanelets.size(); i++) {
     const auto ll = lanelets.at(i);
     const auto lane_id = ll.id();
@@ -126,7 +126,7 @@ void IntersectionModuleManager::launchNewModules(
 void MergeFromPrivateModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  const auto lanelets = getLaneletsOnPath(path, planner_data_->lanelet_map);
+  const auto lanelets = getLaneletsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr());
   for (size_t i = 0; i < lanelets.size(); i++) {
     const auto ll = lanelets.at(i);
     const auto lane_id = ll.id();

planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp

@@ -56,7 +56,8 @@ IntersectionModule::IntersectionModule(
 : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id)
 {
   planner_param_ = planner_param;
-  const auto & assigned_lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id);
+  const auto & assigned_lanelet =
+    planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id);
   turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else");
   has_traffic_light_ =
     !(assigned_lanelet.regulatoryElementsAs<const lanelet::TrafficLight>().empty());
@@ -85,8 +86,8 @@ bool IntersectionModule::modifyPathVelocity(
   geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose;
 
   /* get lanelet map */
-  const auto lanelet_map_ptr = planner_data_->lanelet_map;
-  const auto routing_graph_ptr = planner_data_->routing_graph;
+  const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
+  const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
 
   /* get detection area and conflicting area */
   std::vector<lanelet::ConstLanelets> detection_area_lanelets;
@@ -603,7 +604,7 @@ bool IntersectionModule::checkAngleForTargetLanelets(
   const geometry_msgs::msg::Pose & pose, const std::vector<int> & target_lanelet_ids)
 {
   for (const int lanelet_id : target_lanelet_ids) {
-    const auto ll = planner_data_->lanelet_map->laneletLayer.get(lanelet_id);
+    const auto ll = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lanelet_id);
     if (!lanelet::utils::isInLanelet(pose, ll, planner_param_.detection_area_margin)) {
       continue;
     }

planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp

@@ -61,8 +61,8 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
   geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose;
 
   /* get lanelet map */
-  const auto lanelet_map_ptr = planner_data_->lanelet_map;
-  const auto routing_graph_ptr = planner_data_->routing_graph;
+  const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr();
+  const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr();
 
   /* get detection area */
   std::vector<lanelet::ConstLanelets> detection_area_lanelets;

planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp

@@ -265,7 +265,8 @@ bool getStopPoseIndexFromMap(
   const std::shared_ptr<const PlannerData> & planner_data, int & stop_idx_ip, int dist_thr,
   const rclcpp::Logger logger)
 {
-  lanelet::ConstLanelet lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id);
+  lanelet::ConstLanelet lanelet =
+    planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id);
   const auto road_markings = lanelet.regulatoryElementsAs<lanelet::autoware::RoadMarking>();
   lanelet::ConstLineStrings3d stop_line;
   for (const auto & road_marking : road_markings) {

planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp

@@ -80,7 +80,7 @@ void NoStoppingAreaModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
   for (const auto & no_stopping_area :
-       getNoStoppingAreaRegElemsOnPath(path, planner_data_->lanelet_map)) {
+       getNoStoppingAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
     // Use lanelet_id to unregister module when the route is changed
     const auto module_id = no_stopping_area->id();
     if (!isModuleRegistered(module_id)) {
@@ -97,7 +97,7 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
   const auto no_stopping_area_id_set =
-    getNoStoppingAreaIdSetOnPath(path, planner_data_->lanelet_map);
+    getNoStoppingAreaIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr());
 
   return [no_stopping_area_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
     return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0;

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp

@@ -60,10 +60,8 @@ bool OcclusionSpotModule::modifyPathVelocity(
       0.0);
   }
   const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose;
-  const auto & lanelet_map_ptr = planner_data_->lanelet_map;
-  const auto & traffic_rules_ptr = planner_data_->traffic_rules;
   const auto & occ_grid_ptr = planner_data_->occupancy_grid;
-  if (!lanelet_map_ptr || !traffic_rules_ptr || !occ_grid_ptr) {
+  if (!occ_grid_ptr) {
     return true;
   }
 

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp

@@ -60,12 +60,9 @@ bool OcclusionSpotInPublicModule::modifyPathVelocity(
       0.0);
   }
   const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose;
-  const auto & lanelet_map_ptr = planner_data_->lanelet_map;
-  const auto & routing_graph_ptr = planner_data_->routing_graph;
-  const auto & traffic_rules_ptr = planner_data_->traffic_rules;
   const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects;
 
-  if (!lanelet_map_ptr || !traffic_rules_ptr || !dynamic_obj_arr_ptr || !routing_graph_ptr) {
+  if (!dynamic_obj_arr_ptr) {
     return true;
   }
   PathWithLaneId clipped_path;

planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp

@@ -94,7 +94,8 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node)
 void StopLineModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  for (const auto & stop_line : getStopLinesOnPath(path, planner_data_->lanelet_map)) {
+  for (const auto & stop_line :
+       getStopLinesOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
     const auto module_id = stop_line.id();
     if (!isModuleRegistered(module_id)) {
       registerModule(std::make_shared<StopLineModule>(
@@ -107,7 +108,8 @@ std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
 StopLineModuleManager::getModuleExpiredFunction(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  const auto stop_line_id_set = getStopLineIdSetOnPath(path, planner_data_->lanelet_map);
+  const auto stop_line_id_set =
+    getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr());
 
   return [stop_line_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
     return stop_line_id_set.count(scene_module->getModuleId()) == 0;

planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp

@@ -123,7 +123,7 @@ void TrafficLightModuleManager::launchNewModules(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
   for (const auto & traffic_light_reg_elem :
-       getTrafficLightRegElemsOnPath(path, planner_data_->lanelet_map)) {
+       getTrafficLightRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) {
     const auto stop_line = traffic_light_reg_elem.first->stopLine();
 
     if (!stop_line) {
@@ -147,7 +147,8 @@ std::function<bool(const std::shared_ptr<SceneModuleInterface> &)>
 TrafficLightModuleManager::getModuleExpiredFunction(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path)
 {
-  const auto lanelet_id_set = getLaneletIdSetOnPath(path, planner_data_->lanelet_map);
+  const auto lanelet_id_set =
+    getLaneletIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr());
 
   return [lanelet_id_set](const std::shared_ptr<SceneModuleInterface> & scene_module) {
     return lanelet_id_set.count(scene_module->getModuleId()) == 0;