diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp index ca09b1cd7d422..2396bb16dc8f5 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -37,6 +37,21 @@ class IntersectionModuleManager : public SceneModuleManagerInterface private: IntersectionModule::PlannerParam intersection_param_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +class MergeFromPrivateModuleManager : public SceneModuleManagerInterface +{ +public: + explicit MergeFromPrivateModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "merge_from_private"; } + +private: MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index 407d9715c65b7..e38efd3f6bc9a 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -99,7 +99,9 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface public: struct PlannerParam { - IntersectionModule::PlannerParam intersection_param; + double decel_velocity; + double detection_area_length; + double stop_line_margin; double stop_duration_sec; }; diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 77342c1e6219a..dc647df54f533 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -52,7 +52,7 @@ bool hasDuplicatedPoint( */ bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const IntersectionModule::PlannerParam & planner_param, + const int lane_id, const double detection_area_length, std::vector * conflicting_lanelets_result, std::vector * objective_lanelets_result, const rclcpp::Logger logger); @@ -69,8 +69,7 @@ bool getObjectiveLanelets( */ bool generateStopLine( const int lane_id, const std::vector detection_areas, - const std::shared_ptr & planner_data, - const IntersectionModule::PlannerParam & planner_param, + const std::shared_ptr & planner_data, const double stop_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger); diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 0fba77d1c406a..06060d51d8c84 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -135,7 +135,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_manager_.launchSceneModule(std::make_shared(*this)); } if (this->declare_parameter("launch_intersection", true)) { + // intersection module should be before merge from private to declare intersection parameters planner_manager_.launchSceneModule(std::make_shared(*this)); + planner_manager_.launchSceneModule(std::make_shared(*this)); } if (this->declare_parameter("launch_blind_spot", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp index d346856e8eadc..13e06e2dfe7cc 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -150,7 +150,8 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( } visualization_msgs::msg::MarkerArray createVirtualStopWallMarkerArray( - const geometry_msgs::msg::Pose & pose, const int64_t lane_id, const std::string & stop_factor) + const geometry_msgs::msg::Pose & pose, const int64_t lane_id, const std::string & stop_factor, + const double offset_z = 0.0) { visualization_msgs::msg::MarkerArray msg; @@ -175,7 +176,7 @@ visualization_msgs::msg::MarkerArray createVirtualStopWallMarkerArray( marker_factor_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; marker_factor_text.action = visualization_msgs::msg::Marker::ADD; marker_factor_text.pose = pose; - marker_factor_text.pose.position.z += 2.0; + marker_factor_text.pose.position.z += 2.0 + offset_z; marker_factor_text.scale = createMarkerScale(0.0, 0.0, 1.0); marker_factor_text.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); marker_factor_text.text = stop_factor; @@ -345,7 +346,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark appendMarkerArray( createVirtualStopWallMarkerArray( - debug_data_.virtual_wall_pose, lane_id_, "merge_from_private_road"), + debug_data_.virtual_wall_pose, lane_id_, "merge_from_private_road", -1.0), current_time, &debug_marker_array); } 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 0d235d339af54..b9e88e71a1287 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -83,10 +83,18 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0); ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0); ip.collision_end_margin_time = node.declare_parameter(ns + ".collision_end_margin_time", 2.0); +} + +MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); auto & mp = merge_from_private_area_param_; mp.stop_duration_sec = node.declare_parameter(ns + ".merge_from_private_area.stop_duration_sec", 1.0); - mp.intersection_param = intersection_param_; + mp.decel_velocity = node.get_parameter("intersection.decel_velocity").as_double(); + mp.detection_area_length = node.get_parameter("intersection.detection_area_length").as_double(); + mp.stop_line_margin = node.get_parameter("intersection.stop_line_margin").as_double(); } void IntersectionModuleManager::launchNewModules( @@ -102,6 +110,32 @@ void IntersectionModuleManager::launchNewModules( continue; } + // Is intersection? + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + registerModule(std::make_shared( + module_id, lane_id, planner_data_, intersection_param_, + logger_.get_child("intersection_module"), clock_)); + } +} + +void MergeFromPrivateModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lanelets = getLaneletsOnPath(path, planner_data_->lanelet_map); + for (size_t i = 0; i < lanelets.size(); i++) { + const auto ll = lanelets.at(i); + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + if (isModuleRegistered(module_id)) { + continue; + } + // Is intersection? const std::string turn_direction = ll.attributeOr("turn_direction", "else"); const auto is_intersection = @@ -121,10 +155,6 @@ void IntersectionModuleManager::launchNewModules( logger_.get_child("merge_from_private_road_module"), clock_)); } } - - registerModule(std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, - logger_.get_child("intersection_module"), clock_)); } } @@ -138,4 +168,14 @@ IntersectionModuleManager::getModuleExpiredFunction( return lane_id_set.count(scene_module->getModuleId()) == 0; }; } +std::function &)> +MergeFromPrivateModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_id_set = getLaneIdSetOnPath(path); + + return [lane_id_set](const std::shared_ptr & scene_module) { + return lane_id_set.count(scene_module->getModuleId()) == 0; + }; +} } // namespace behavior_velocity_planner 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 576b769ec8a78..dc813aded1d72 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 @@ -94,8 +94,8 @@ bool IntersectionModule::modifyPathVelocity( std::vector conflicting_area_lanelets; util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_, &conflicting_area_lanelets, - &detection_area_lanelets, logger_); + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, + &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( conflicting_area_lanelets, planner_param_.detection_area_length); std::vector detection_areas = util::getPolygon3dFromLaneletsVec( @@ -116,8 +116,8 @@ bool IntersectionModule::modifyPathVelocity( int pass_judge_line_idx = -1; int first_idx_inside_lane = -1; if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_, path, *path, &stop_line_idx, - &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, *path, + &stop_line_idx, &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); RCLCPP_DEBUG(logger_, "===== plan end ====="); return false; 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 00af9463f3f02..34ab9dbbee92d 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 @@ -69,10 +69,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( std::vector conflicting_area_lanelets; util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.intersection_param, + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, &conflicting_area_lanelets, &detection_area_lanelets, logger_); std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( - conflicting_area_lanelets, planner_param_.intersection_param.detection_area_length); + conflicting_area_lanelets, planner_param_.detection_area_length); if (conflicting_areas.empty()) { RCLCPP_DEBUG(logger_, "no detection area. skip computation."); return true; @@ -86,7 +86,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( const auto private_path = extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); if (!util::generateStopLine( - lane_id_, conflicting_areas, planner_data_, planner_param_.intersection_param, path, + lane_id_, conflicting_areas, planner_data_, planner_param_.stop_line_margin, path, private_path, &stop_line_idx, &judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); @@ -108,7 +108,7 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity( /* set stop speed */ if (state_machine_.getState() == State::STOP) { constexpr double stop_vel = 0.0; - const double decel_vel = planner_param_.intersection_param.decel_velocity; + const double decel_vel = planner_param_.decel_velocity; double v = (has_traffic_light_ && turn_direction_ == "straight") ? decel_vel : stop_vel; util::setVelocityFrom(stop_line_idx, v, path); 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 f9f68fb4bea7c..e4f2b818fbdaf 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -162,8 +162,7 @@ int getFirstPointInsidePolygons( bool generateStopLine( const int lane_id, const std::vector detection_areas, - const std::shared_ptr & planner_data, - const IntersectionModule::PlannerParam & planner_param, + const std::shared_ptr & planner_data, const double stop_line_margin, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger) @@ -178,7 +177,7 @@ bool generateStopLine( /* set parameters */ constexpr double interval = 0.2; - const int margin_idx_dist = std::ceil(planner_param.stop_line_margin / interval); + const int margin_idx_dist = std::ceil(stop_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); @@ -322,7 +321,7 @@ bool getStopPoseIndexFromMap( bool getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id, const IntersectionModule::PlannerParam & planner_param, + const int lane_id, const double detection_area_length, std::vector * conflicting_lanelets_result, std::vector * objective_lanelets_result, const rclcpp::Logger logger) { @@ -379,7 +378,7 @@ bool getObjectiveLanelets( } // get possible lanelet path that reaches conflicting_lane longer than given length - const double length = planner_param.detection_area_length; + const double length = detection_area_length; std::vector objective_lanelets_sequences; for (const auto & ll : objective_lanelets) { // Preceding lanes does not include objective_lane so add them at the end