adds: timer to NAV loop

ut-amrl · Sep 27, 2024 · da99206 · da99206
1 parent ad57395
commit da99206
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 2 deletions.
diff --git a/src/navigation/navigation.cc b/src/navigation/navigation.cc
@@ -831,7 +831,7 @@ vector<GraphDomain::State> Navigation::GetPlanPath() {
   return plan_path_;
 }
 
-bool Navigation::Run(const double& time,
+bool Navigation::RunInternal(const double& time,
                      Vector2f& cmd_vel,
                      float& cmd_angle_vel) {
   const bool kDebug = FLAGS_v > 0;
@@ -956,4 +956,18 @@ bool Navigation::Run(const double& time,
   return true;
 }
 
-}  // namespace navigation
+bool Navigation::Run(const double& time, Vector2f& cmd_vel, float& cmd_angle_vel) {
+  auto start_run_loop = std::chrono::system_clock::now();
+  bool retval = Navigation::RunInternal(time, cmd_vel, cmd_angle_vel);
+  auto end_run_loop = std::chrono::system_clock::now();
+  std::chrono::duration<double, std::milli> time_diff = end_run_loop - start_run_loop;
+
+  printf(
+    "LOOP: %ld,%f\n", 
+    std::chrono::duration_cast<std::chrono::milliseconds>(start_run_loop.time_since_epoch()).count(),
+    time_diff.count()
+  );
+  return retval;
+}
+
+}  // namespace navigation
diff --git a/src/navigation/navigation.h b/src/navigation/navigation.h
@@ -91,6 +91,8 @@ class Navigation {
                          double time);
   void ObserveImage(cv::Mat image, double time);
   bool Run(const double& time, Eigen::Vector2f& cmd_vel, float& cmd_angle_vel);
+  bool RunInternal(const double& time, Eigen::Vector2f& cmd_vel, float& cmd_angle_vel);
+
   void GetStraightFreePathLength(float* free_path_length,
                                  float* clearance);
   void GetFreePathLength(float curvature,