feat: output avoidance debug array whenever the avoidance module is e…

…nabled (tier4#835) Signed-off-by: tomoya.kimura <[email protected]>
boyali · Oct 19, 2022 · 1126de4 · 1126de4
1 parent 0cbda12
commit 1126de4
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 22 deletions.
diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -157,8 +157,7 @@ class AvoidanceModule : public SceneModuleInterface
   // debug
   mutable DebugData debug_data_;
   void setDebugData(const PathShifter & shifter, const DebugData & debug);
-  mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_object;
-  mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_point;
+  mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_point_;
   // =====================================
   // ========= helper functions ==========
   // =====================================

diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp
@@ -129,7 +129,7 @@ AvoidanceDebugMsgArray BehaviorTreeManager::getAvoidanceDebugMsgArray()
   const auto avoidance_module = std::find_if(
     scene_modules_.begin(), scene_modules_.end(),
     [](const std::shared_ptr<SceneModuleInterface> & module_ptr) {
-      return module_ptr->current_state_ == BT::NodeStatus::SUCCESS;
+      return module_ptr->name() == "Avoidance";
     });
   if (avoidance_module != scene_modules_.end()) {
     const auto & ptr = avoidance_module->get()->getAvoidanceDebugMsgArray();

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -77,8 +77,6 @@ bool AvoidanceModule::isExecutionReady() const
   {
     DebugData debug;
     static_cast<void>(calcAvoidancePlanningData(debug));
-    debug_avoidance_msg_array_ptr_ =
-      std::make_shared<AvoidanceDebugMsgArray>(debug.avoidance_debug_msg_array);
   }
 
   if (current_state_ == BT::NodeStatus::RUNNING) {
@@ -95,8 +93,6 @@ BT::NodeStatus AvoidanceModule::updateState()
   DebugData debug;
   const auto avoid_data = calcAvoidancePlanningData(debug);
   const bool has_avoidance_target = !avoid_data.objects.empty();
-  debug_avoidance_msg_array_ptr_ =
-    std::make_shared<AvoidanceDebugMsgArray>(debug.avoidance_debug_msg_array);
   if (!is_plan_running && !has_avoidance_target) {
     current_state_ = BT::NodeStatus::SUCCESS;
   } else {
@@ -298,8 +294,13 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
 
   // debug
   {
-    debug_avoidance_initializer_for_object = avoidance_debug_msg_array;
-    debug.avoidance_debug_msg_array.avoidance_info = std::move(avoidance_debug_msg_array);
+    auto & debug_data_avoidance = debug_data_.avoidance_debug_msg_array.avoidance_info;
+    debug_data_avoidance = avoidance_debug_msg_array;
+    debug_data_avoidance.insert(
+      debug_data_avoidance.end(), debug_avoidance_initializer_for_shift_point_.begin(),
+      debug_avoidance_initializer_for_shift_point_.end());
+    debug_avoidance_msg_array_ptr_ =
+      std::make_shared<AvoidanceDebugMsgArray>(debug_data_.avoidance_debug_msg_array);
     debug.farthest_linestring_from_overhang =
       std::make_shared<lanelet::ConstLineStrings3d>(debug_linestring);
     debug.current_lanelets = std::make_shared<lanelet::ConstLanelets>(current_lanes);
@@ -595,14 +596,7 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
     avoidance_debug_msg_array.push_back(avoidance_debug_msg);
   }
 
-  {
-    debug_avoidance_initializer_for_shift_point = std::move(avoidance_debug_msg_array);
-    auto & debug_data_avoidance = debug_data_.avoidance_debug_msg_array.avoidance_info;
-    debug_data_avoidance = debug_avoidance_initializer_for_object;
-    debug_data_avoidance.insert(
-      debug_data_avoidance.end(), debug_avoidance_initializer_for_shift_point.begin(),
-      debug_avoidance_initializer_for_shift_point.end());
-  }
+  debug_avoidance_initializer_for_shift_point_ = std::move(avoidance_debug_msg_array);
   fillAdditionalInfoFromLongitudinal(avoid_points);
 
   return avoid_points;
@@ -2177,8 +2171,6 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval()
   // we can execute the plan() since it handles the approval appropriately.
   BehaviorModuleOutput out = plan();
   out.path_candidate = std::make_shared<PathWithLaneId>(planCandidate());
-  debug_avoidance_msg_array_ptr_ =
-    std::make_shared<AvoidanceDebugMsgArray>(debug_data_.avoidance_debug_msg_array);
   return out;
 }
 
@@ -2401,9 +2393,6 @@ void AvoidanceModule::updateData()
 {
   debug_data_ = DebugData();
   avoidance_data_ = calcAvoidancePlanningData(debug_data_);
-  const auto avoidance_debug_msgs = debug_data_.avoidance_debug_msg_array.avoidance_info;
-  debug_avoidance_msg_array_ptr_ =
-    std::make_shared<AvoidanceDebugMsgArray>(debug_data_.avoidance_debug_msg_array);
 
   // TODO(Horibe): this is not tested yet, disable now.
   updateRegisteredObject(avoidance_data_.objects);
@@ -2551,6 +2540,7 @@ void AvoidanceModule::initVariables()
   path_shifter_ = PathShifter{};
 
   debug_avoidance_msg_array_ptr_.reset();
+  debug_avoidance_initializer_for_shift_point_.clear();
   debug_data_ = DebugData();
 
   registered_raw_shift_points_ = {};