chore(behavior_velocity): add debug cout

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
autowarefoundation · Feb 21, 2022 · d12056d · d12056d
1 parent 2f49b89
commit d12056d
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 0 deletions.
diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp
@@ -35,6 +35,7 @@ namespace behavior_velocity_planner
 {
 class StopLineModule : public SceneModuleInterface
 {
+  int counter = 0;
   using StopLineWithLaneId = std::pair<lanelet::ConstLineString3d, int64_t>;
 
 public:

diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp
@@ -265,6 +265,12 @@ bool StopLineModule::modifyPathVelocity(
     }
   }
 
+  if (counter % 50 == 0) {
+    std::cout << "path start_idx: " << start_idx << " stop_idx: " << stop_pose_with_index->index
+              << " stop_line_id: " << lane_id_ << " arclength: " << signed_arc_dist_to_stop_point
+              << " current_state: " << static_cast<int>(state_) << std::endl;
+  }
+  counter++;
   return true;
 }