diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 472d59a4d9a0d..67592fdf8a33e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -338,8 +338,9 @@ std::pair 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); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index a634d095f63a5..1a8701ff1672d 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -422,7 +422,7 @@ std::pair 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 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()) { diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 195b482708a21..06242c9e92cdf 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -353,8 +353,9 @@ std::pair 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); diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp index da9896b898ca0..aabddf6ba6836 100644 --- a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp +++ b/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 #include @@ -69,16 +71,11 @@ struct PlannerData std::deque velocity_buffer; autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; - lanelet::LaneletMapPtr lanelet_map; // occupancy grid nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; // other internal data std::map traffic_light_id_map; - lanelet::traffic_rules::TrafficRulesPtr traffic_rules; - lanelet::routing::RoutingGraphPtr routing_graph; - std::shared_ptr overall_graphs; - // external data std::map external_traffic_light_id_map; @@ -86,6 +83,8 @@ struct PlannerData boost::optional external_intersection_status_input; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; + // route handler + std::shared_ptr route_handler_; // parameters vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 2c835429311e8..a69c2b04733a9 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -27,6 +27,7 @@ pcl_conversions rclcpp rclcpp_components + route_handler sensor_msgs tf2 tf2_eigen diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 06060d51d8c84..9bc3c94624a96 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/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::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(overall_graphs); - } + planner_data_.route_handler_ = std::make_shared(*msg); } void BehaviorVelocityPlannerNode::onTrafficSignals( diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp index f4edbc1b737a2..6189c003eb926 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp +++ b/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; diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 3070e4b4ed82f..b0e8d4c8b74b8 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/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 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; } diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp index d4425eb8ff917..52d2da760d575 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp +++ b/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( @@ -112,8 +113,9 @@ std::function &)> 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 & scene_module) { return crosswalk_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp index 6fc8498a79d4a..1f6b8d9a9eb46 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp @@ -175,7 +175,8 @@ lanelet::Optional getStopLineFromMap( const int lane_id, const std::shared_ptr & 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::ConstLineStrings3d stop_line; for (const auto & road_marking : road_markings) { diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp index f07106d6ea1af..8dc92269a4e1d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/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 &)> 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 & scene_module) { return detection_area_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index b9e88e71a1287..b46f18a139a70 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/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(); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 786d44190af87..9e2e86ccfe421 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/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().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 detection_area_lanelets; @@ -603,7 +604,7 @@ bool IntersectionModule::checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const std::vector & 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; } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index 36dd26cef593c..92f42ed7a2905 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/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 detection_area_lanelets; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index 6dbe52073ae53..b0a91cba575f3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -265,7 +265,8 @@ bool getStopPoseIndexFromMap( const std::shared_ptr & 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::ConstLineStrings3d stop_line; for (const auto & road_marking : road_markings) { diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp index df372e73e1406..82e4211f52c6f 100644 --- a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp +++ b/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 & scene_module) { return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp index ad7d7da0f428a..426a2efbfbf6e 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot.cpp +++ b/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; } diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp index 60e2d211e9ca6..a351bce4a62c3 100644 --- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp +++ b/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; diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp index ebb038c191f85..7c78395470e73 100644 --- a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp +++ b/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( @@ -107,7 +108,8 @@ std::function &)> 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 & scene_module) { return stop_line_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index bb82168f170dd..2b2d8dff8b36a 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/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 &)> 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 & scene_module) { return lanelet_id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index c637ffbd73ef0..7ac6918f13ed2 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -81,8 +81,8 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node void VirtualTrafficLightModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - for (const auto & m : - getRegElemMapOnPath(path, planner_data_->lanelet_map)) { + for (const auto & m : getRegElemMapOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed const auto module_id = m.second.id(); if (!isModuleRegistered(module_id)) { @@ -97,7 +97,8 @@ std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - const auto id_set = getLaneletIdSetOnPath(path, planner_data_->lanelet_map); + const auto id_set = + getLaneletIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); return [id_set](const std::shared_ptr & scene_module) { return id_set.count(scene_module->getModuleId()) == 0; diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 8eedd18786a4f..af7fe6c90bced 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -73,7 +73,13 @@ class RouteHandler bool isHandlerReady() const; lanelet::ConstPolygon3d getExtraDrivableAreaById(const lanelet::Id id) const; Header getRouteHeader() const; - lanelet::routing::RoutingGraphContainer getOverallGraph() const; + + // for routing graph + bool isMapMsgReady() const; + lanelet::routing::RoutingGraphPtr getRoutingGraphPtr() const; + lanelet::traffic_rules::TrafficRulesPtr getTrafficRulesPtr() const; + std::shared_ptr getOverallGraphPtr() const; + lanelet::LaneletMapPtr getLaneletMapPtr() const; // for routing bool planPathLaneletsBetweenCheckpoints( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index be4a14173f7e1..afc5d9c17e6b7 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1271,11 +1271,26 @@ lanelet::ConstLanelets RouteHandler::getCheckTargetLanesFromPath( return check_lanelets; } -lanelet::routing::RoutingGraphContainer RouteHandler::getOverallGraph() const +bool RouteHandler::isMapMsgReady() const { return is_map_msg_ready_; } + +lanelet::routing::RoutingGraphPtr RouteHandler::getRoutingGraphPtr() const +{ + return routing_graph_ptr_; +} + +lanelet::traffic_rules::TrafficRulesPtr RouteHandler::getTrafficRulesPtr() const { - return *overall_graphs_ptr_; + return traffic_rules_ptr_; } +std::shared_ptr RouteHandler::getOverallGraphPtr() + const +{ + return overall_graphs_ptr_; +} + +lanelet::LaneletMapPtr RouteHandler::getLaneletMapPtr() const { return lanelet_map_ptr_; } + lanelet::routing::RelationType RouteHandler::getRelation( const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const {