diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 19c7145073051..45bc12049ba33 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -157,6 +157,7 @@ class IntersectionModule : public SceneModuleInterface { occlusion_first_stop_activated_ = activation; } + bool isOccluded() const { return is_occluded_; } private: rclcpp::Node & node_; @@ -173,19 +174,19 @@ class IntersectionModule : public SceneModuleInterface // for occlusion detection const bool enable_occlusion_detection_; std::optional> detection_divisions_; - std::optional prev_occlusion_stop_line_pose_; - OcclusionState occlusion_state_; + bool is_occluded_ = false; + OcclusionState occlusion_state_ = OcclusionState::NONE; // NOTE: uuid_ is base member // for occlusion clearance decision const UUID occlusion_uuid_; - bool occlusion_safety_; + bool occlusion_safety_ = true; double occlusion_stop_distance_; - bool occlusion_activated_; + bool occlusion_activated_ = true; // for first stop in two-phase stop const UUID occlusion_first_stop_uuid_; // TODO(Mamoru Sobue): replace with uuid_ - bool occlusion_first_stop_safety_; + bool occlusion_first_stop_safety_ = true; double occlusion_first_stop_distance_; - bool occlusion_first_stop_activated_; + bool occlusion_first_stop_activated_ = true; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index bc023491a1975..4f6903ded6767 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -88,6 +88,7 @@ tier4_v2x_msgs vehicle_info_util visualization_msgs + grid_map_rviz_plugin ament_cmake_ros ament_lint_auto 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 88e861be51db4..5eb57f06ec372 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -203,7 +203,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createVirtualWallMarker const auto now = this->clock_->now(); - // int32_t uid = planning_utils::bitShift(module_id_); // TODO(Mamoru Sobue): collision stop pose depends on before/after occlusion clearance if (!activated_) { appendMarkerArray( 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 0605486da3ea8..66acc4d92e013 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -127,20 +127,8 @@ void IntersectionModuleManager::launchNewModules( const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, assoc_ids, enable_occlusion_detection, node_, logger_.get_child("intersection_module"), clock_); - const auto occlusion_uuid = new_module->getOcclusionUUID(); - const auto occlusion_first_stop_uuid = new_module->getOcclusionFirstStopUUID(); registerModule(std::move(new_module)); - // default generateUUID(module_id); - updateRTCStatus( - getUUID(module_id), true, std::numeric_limits::lowest(), path.header.stamp); - // occlusion - occlusion_rtc_interface_.updateCooperateStatus( - occlusion_uuid, true, std::numeric_limits::lowest(), - std::numeric_limits::lowest(), path.header.stamp); - rtc_interface_.updateCooperateStatus( - occlusion_first_stop_uuid, true, std::numeric_limits::lowest(), - std::numeric_limits::lowest(), path.header.stamp); enable_occlusion_detection = false; } @@ -188,23 +176,36 @@ bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegister void IntersectionModuleManager::sendRTC(const Time & stamp) { for (const auto & scene_module : scene_modules_) { - // default - const UUID uuid = getUUID(scene_module->getModuleId()); - updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); - // occlusion const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const bool is_occluded = intersection_module->isOccluded(); + const UUID uuid = getUUID(scene_module->getModuleId()); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); - const auto occlusion_safety = intersection_module->getOcclusionSafety(); - const auto occlusion_distance = intersection_module->getOcclusionDistance(); - occlusion_rtc_interface_.updateCooperateStatus( - occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); - const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID(); - const auto occlusion_first_stop_safety = intersection_module->getOcclusionFirstStopSafety(); - const auto occlusion_first_stop_distance = intersection_module->getOcclusionFirstStopDistance(); - rtc_interface_.updateCooperateStatus( - occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance, - occlusion_first_stop_distance, stamp); + if (!is_occluded) { + rtc_interface_.clearCooperateStatus(); + // default + updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, true, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), stamp); + // send {default, occlusion} RTC status + } else { + rtc_interface_.clearCooperateStatus(); + // occlusion + const auto occlusion_safety = intersection_module->getOcclusionSafety(); + const auto occlusion_distance = intersection_module->getOcclusionDistance(); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); + + const auto occlusion_first_stop_safety = intersection_module->getOcclusionFirstStopSafety(); + const auto occlusion_first_stop_distance = + intersection_module->getOcclusionFirstStopDistance(); + rtc_interface_.updateCooperateStatus( + occlusion_first_stop_uuid, occlusion_first_stop_safety, occlusion_first_stop_distance, + occlusion_first_stop_distance, stamp); + // send {first_stop, occlusion} RTC status + // rtc_interface_.removeCooperateStatus(uuid); + } } rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() occlusion_rtc_interface_.publishCooperateStatus(stamp); @@ -213,16 +214,22 @@ void IntersectionModuleManager::sendRTC(const Time & stamp) void IntersectionModuleManager::setActivation() { for (const auto & scene_module : scene_modules_) { - // default - scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); - // occlusion const auto intersection_module = std::dynamic_pointer_cast(scene_module); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); - intersection_module->setOcclusionActivation( - occlusion_rtc_interface_.isActivated(occlusion_uuid)); const auto occlusion_first_stop_uuid = intersection_module->getOcclusionFirstStopUUID(); - intersection_module->setOcclusionFirstStopActivation( - rtc_interface_.isActivated(occlusion_first_stop_uuid)); + const bool is_occluded = intersection_module->isOccluded(); + if (!is_occluded) { + // default + scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); + intersection_module->setOcclusionActivation( + occlusion_rtc_interface_.isActivated(occlusion_uuid)); + } else { + // occlusion + intersection_module->setOcclusionActivation( + occlusion_rtc_interface_.isActivated(occlusion_uuid)); + intersection_module->setOcclusionFirstStopActivation( + rtc_interface_.isActivated(occlusion_first_stop_uuid)); + } } } 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 186c03bcce854..1c9e235547c30 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 @@ -67,12 +67,9 @@ IntersectionModule::IntersectionModule( : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), - is_go_out_(false), assoc_ids_(assoc_ids), enable_occlusion_detection_(enable_occlusion_detection), detection_divisions_(std::nullopt), - prev_occlusion_stop_line_pose_(std::nullopt), - occlusion_state_(OcclusionState::NONE), occlusion_uuid_(tier4_autoware_utils::generateUUID()), occlusion_first_stop_uuid_(tier4_autoware_utils::generateUUID()) { @@ -289,8 +286,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * if (!default_stop_line_idx_opt) { occlusion_stop_required = true; stop_line_idx = occlusion_stop_line_idx = occlusion_stop_line_idx_opt; - prev_occlusion_stop_line_pose_ = - path_ip.points.at(occlusion_stop_line_idx_opt.value()).point.pose; + is_occluded_ = true; occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE; RCLCPP_WARN(logger_, "directly stop at occlusion stop line because collision line not found"); } else if (before_creep_state_machine_.getState() == StateMachine::State::GO) { @@ -300,8 +296,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // insert creep velocity [closest_idx, occlusion_stop_line) insert_creep_during_occlusion = std::make_pair(closest_idx, occlusion_stop_line_idx_opt.value()); - prev_occlusion_stop_line_pose_ = - path_ip.points.at(occlusion_stop_line_idx_ip_opt.value()).point.pose; + is_occluded_ = true; occlusion_state_ = OcclusionState::CREEP_SECOND_STOP_LINE; } else { const double dist_default_stop_line = motion_utils::calcSignedArcLength( @@ -320,32 +315,32 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // insert creep velocity [default_stop_line, occlusion_stop_line) insert_creep_during_occlusion = std::make_pair(default_stop_line_idx_opt.value(), occlusion_stop_line_idx_opt.value()); - prev_occlusion_stop_line_pose_ = - path_ip.points.at(occlusion_stop_line_idx_ip_opt.value()).point.pose; + is_occluded_ = true; occlusion_state_ = OcclusionState::BEFORE_FIRST_STOP_LINE; } - } else if (prev_occlusion_stop_line_pose_) { + } else if (is_occluded_) { // previously occlusion existed, but now it is clear - const auto prev_occlusion_stop_pose_idx = motion_utils::findNearestIndex( - path->points, prev_occlusion_stop_line_pose_.value(), 3.0, M_PI_4); if (!util::isOverTargetIndex( *path, closest_idx, current_pose, default_stop_line_idx_opt.value())) { stop_line_idx = default_stop_line_idx_opt.value(); - prev_occlusion_stop_line_pose_ = std::nullopt; - } else if (!util::isOverTargetIndex( - *path, closest_idx, current_pose, prev_occlusion_stop_pose_idx.get())) { - stop_line_idx = prev_occlusion_stop_pose_idx.get(); - } else { - // TODO(Mamoru Sobue): consider static occlusion limit stop line - prev_occlusion_stop_line_pose_ = std::nullopt; + } else if ( + static_pass_judge_line_opt && + !util::isOverTargetIndex( + *path, closest_idx, current_pose, static_pass_judge_line_opt.value())) { + stop_line_idx = static_pass_judge_line_opt; } occlusion_state_ = OcclusionState::CLEARED; + is_occluded_ = false; if (stop_line_idx && has_collision) { // do collision checking at previous occlusion stop line collision_stop_required = true; } else { collision_stop_required = false; } + if (is_stuck && stuck_line_idx_opt) { + stuck_stop_required = true; + stop_line_idx = static_pass_judge_line_opt; + } } else if (is_stuck && stuck_line_idx_opt) { stuck_stop_required = true; const size_t stuck_line_idx = stuck_line_idx_opt.value(); @@ -401,7 +396,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } /* make decision */ - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const double baselink2front = planner_data_->vehicle_info_.vehicle_length_m; if (!occlusion_activated_) { is_go_out_ = false; /* in case of creeping */