diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 4e741a2b3db2c..c41c3658b90f9 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -96,25 +96,6 @@ class LaneChangeInterface : public SceneModuleInterface TurnSignalInfo getCurrentTurnSignalInfo( const PathWithLaneId & path, const TurnSignalInfo & original_turn_signal_info); - // TODO(someone): remove this, and use base class function - [[deprecated]] BehaviorModuleOutput run() override - { - updateData(); - - if (!isWaitingApproval()) { - return plan(); - } - - // module is waiting approval. Check it. - if (isActivated()) { - RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); - return plan(); - } else { - RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); - return planWaitingApproval(); - } - } - protected: std::shared_ptr parameters_; @@ -135,12 +116,16 @@ class LaneChangeInterface : public SceneModuleInterface void updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const; + void debug_print(std::string_view message) const; + mutable MarkerArray virtual_wall_marker_; std::unique_ptr prev_approved_path_; void clearAbortApproval() { is_abort_path_approved_ = false; } + void initRTCStatus(); + bool is_abort_path_approved_{false}; bool is_abort_approval_requested_{false}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 451899fbf27e6..bba8351292188 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -174,6 +174,8 @@ class LaneChangeBase bool isValidPath() const { return status_.is_valid_path; } + bool hasNoTargetLanes() const { return status_.target_lanes.empty(); } + void setData(const std::shared_ptr & data) { planner_data_ = data; } void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; } @@ -210,6 +212,8 @@ class LaneChangeBase void resetStopPose() { lane_change_stop_pose_ = std::nullopt; } + void debug_print(std::string_view message) const { RCLCPP_DEBUG(logger_, "%s", message.data()); } + protected: virtual lanelet::ConstLanelets getCurrentLanes() const = 0; diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 22d85a2a1fc2a..2b182ce19bdc8 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -86,15 +86,21 @@ void LaneChangeInterface::updateData() void LaneChangeInterface::postProcess() { + if (module_type_->hasNoTargetLanes()) { + return; + } + post_process_safety_status_ = module_type_->isApprovedPathSafe(); } BehaviorModuleOutput LaneChangeInterface::plan() { + debug_print(__func__); resetPathCandidate(); resetPathReference(); if (!module_type_->isValidPath()) { + debug_print("no valid path found"); return {}; } @@ -400,6 +406,13 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o SteeringFactor::TURNING, ""); } +void LaneChangeInterface::initRTCStatus() +{ + for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { + uuid_map_.at(module_name) = tier4_autoware_utils::generateUUID(); + } +} + void LaneChangeInterface::updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const { @@ -495,4 +508,9 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( // not in the vicinity of the end of the path. return original return original_turn_signal_info; } + +void LaneChangeInterface::debug_print(std::string_view message) const +{ + RCLCPP_DEBUG(getLogger(), "%s", message.data()); +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index c7ddd8fe63636..9fe3c14daf72a 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -145,6 +145,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() BehaviorModuleOutput output; if (isAbortState() && abort_path_) { + debug_print("abort state"); output.path = abort_path_->path; output.reference_path = prev_module_reference_path_; output.turn_signal_info = prev_turn_signal_info_; @@ -351,6 +352,9 @@ PathWithLaneId NormalLaneChange::getReferencePath() const std::optional NormalLaneChange::extendPath() { + if (status_.target_lanes.empty()) { + return std::nullopt; + } const auto path = status_.lane_change_path.path; const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index 1a8c8241abe1a..a7b106fe00051 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -349,16 +349,22 @@ class SceneModuleInterface bool canTransitWaitingApprovalState() const { if (!existRegisteredRequest()) { + RCLCPP_DEBUG(getLogger(), "%s registered request not exist", __func__); return false; } + + RCLCPP_DEBUG(getLogger(), "%s registered request exist", __func__); return existNotApprovedRequest(); } bool canTransitWaitingApprovalToRunningState() const { if (!existRegisteredRequest()) { + RCLCPP_DEBUG(getLogger(), "%s registered request not exist", __func__); return true; } + + RCLCPP_DEBUG(getLogger(), "%s registered request exist", __func__); return existApprovedRequest(); } @@ -524,14 +530,26 @@ class SceneModuleInterface */ bool isActivated() const { + auto log_debug_throttled = [&](std::string_view message) -> void { + RCLCPP_DEBUG(getLogger(), "%s", message.data()); + }; + log_debug_throttled(__func__); if (rtc_interface_ptr_map_.empty()) { + log_debug_throttled("rtc_interface_ptr_map_ is empty."); return true; } if (!existRegisteredRequest()) { + log_debug_throttled("Registered request not exist."); return false; } - return existApprovedRequest(); + if (existApprovedRequest()) { + log_debug_throttled("Approved request exist."); + return true; + } + + log_debug_throttled("Approved request not exist."); + return false; } void removeRTCStatus()