logs

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

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<LaneChangeParameters> 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<PathWithLaneId> prev_approved_path_;
 
   void clearAbortApproval() { is_abort_path_approved_ = false; }
 
+  void initRTCStatus();
+
   bool is_abort_path_approved_{false};
 
   bool is_abort_approval_requested_{false};

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<const PlannerData> & data) { planner_data_ = data; }
 
   void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; }
@@ -210,6 +212,10 @@ 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;
 

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,12 @@ 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
 {

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<PathWithLaneId> 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;
 

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()
@@ -543,6 +561,7 @@ class SceneModuleInterface
     }
   }
 
+
   void setStopReason(const std::string & stop_reason, const PathWithLaneId & path)
   {
     stop_reason_.reason = stop_reason;