fix: behavior path avoidance debug (tier4#1068)

* fix: behavior path avoidance debug Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * change timeout sec Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> * functionize the creation of debug data Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>
boyali · Jul 1, 2022 · b67c5bf · b67c5bf
1 parent 033c77e
commit b67c5bf
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 6 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,7 +157,9 @@ class AvoidanceModule : public SceneModuleInterface
   // debug
   mutable DebugData debug_data_;
   void setDebugData(const PathShifter & shifter, const DebugData & debug);
+  void updateAvoidanceDebugData(std::vector<AvoidanceDebugMsg> & avoidance_debug_msg_array) const;
   mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_point_;
+  mutable rclcpp::Time debug_avoidance_initializer_for_shift_point_time_;
   // =====================================
   // ========= helper functions ==========
   // =====================================

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
@@ -294,11 +294,7 @@ ObjectDataArray AvoidanceModule::calcAvoidanceTargetObjects(
 
   // debug
   {
-    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());
+    updateAvoidanceDebugData(avoidance_debug_msg_array);
     debug_avoidance_msg_array_ptr_ =
       std::make_shared<AvoidanceDebugMsgArray>(debug_data_.avoidance_debug_msg_array);
     debug.farthest_linestring_from_overhang =
@@ -456,6 +452,7 @@ void AvoidanceModule::registerRawShiftPoints(const AvoidPointArray & future)
 AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
   const ObjectDataArray & objects) const
 {
+  debug_avoidance_initializer_for_shift_point_.clear();
   const auto prepare_distance = getNominalPrepareDistance();
 
   // To be consistent with changes in the ego position, the current shift length is considered.
@@ -580,6 +577,7 @@ AvoidPointArray AvoidanceModule::calcRawShiftPointsFromObjects(
   }
 
   debug_avoidance_initializer_for_shift_point_ = std::move(avoidance_debug_msg_array);
+  debug_avoidance_initializer_for_shift_point_time_ = clock_->now();
   fillAdditionalInfoFromLongitudinal(avoid_points);
 
   return avoid_points;
@@ -2523,7 +2521,6 @@ 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_ = {};
@@ -2665,4 +2662,21 @@ void AvoidanceModule::setDebugData(const PathShifter & shifter, const DebugData
   addAvoidPoint(debug.new_shift_points, "path_shifter_proposed_points", 0.99, 0.0, 0.0, 0.5);
 }
 
+void AvoidanceModule::updateAvoidanceDebugData(
+  std::vector<AvoidanceDebugMsg> & avoidance_debug_msg_array) const
+{
+  debug_data_.avoidance_debug_msg_array.avoidance_info.clear();
+  auto & debug_data_avoidance = debug_data_.avoidance_debug_msg_array.avoidance_info;
+  debug_data_avoidance = avoidance_debug_msg_array;
+  if (!debug_avoidance_initializer_for_shift_point_.empty()) {
+    const bool is_info_old_ =
+      (clock_->now() - debug_avoidance_initializer_for_shift_point_time_).seconds() > 0.1;
+    if (!is_info_old_) {
+      debug_data_avoidance.insert(
+        debug_data_avoidance.end(), debug_avoidance_initializer_for_shift_point_.begin(),
+        debug_avoidance_initializer_for_shift_point_.end());
+    }
+  }
+}
+
 }  // namespace behavior_path_planner