add intermed part

Makefile

@@ -20,5 +20,5 @@ clean:
 
 build/CMakeLists.txt.copy: CMakeLists.txt Makefile srv
 	mkdir -p build
-	cd build && cmake -DCMAKE_BUILD_TYPE=$(build_type) ..
+	cd build && cmake -DCMAKE_CXX_STANDARD=17 -DCMAKE_BUILD_TYPE=$(build_type) ..
 	cp CMakeLists.txt build/CMakeLists.txt.copy

automata_navigation_remapped.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+if [ -z "$VERBOSE_LOGGING" ]; then
+    VERBOSE_LOGGING=0
+fi
+
+echo VERBOSE_LOGGING=$VERBOSE_LOGGING
+
+VERBOSE_LOGGING=$VERBOSE_LOGGING ./bin/navigation \
+	/velodyne_2dscan:=/scan \
+	/jackal_velocity_controller/odom:=/odom

src/navigation/navigation.cc

@@ -213,6 +213,14 @@ void Navigation::Initialize(const NavigationParameters& params,
     exit(1);
   }
   evaluator_ = std::unique_ptr<PathEvaluatorBase>(evaluator);
+
+  const char* verbosity = std::getenv("VERBOSE_LOGGING");
+  if(verbosity == nullptr) verbose_logging = false;
+  else {
+    std::string verbosity_str = std::string(verbosity);
+    if(verbosity_str == "1") verbose_logging=true;
+    else verbose_logging=false;
+  }
 }
 
 void Navigation::LoadVectorMap(const string& map_file){ //Assume map is given as MAP.navigation.json
@@ -1166,7 +1174,7 @@ Eigen::Vector2f Navigation::GetIntermediateGoal(){
 }
 
 
-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;
@@ -1463,4 +1471,20 @@ bool Navigation::Run(const double& time,
   return true;
 }
 
+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;
+
+  if(verbose_logging) {
+    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

src/navigation/navigation.h

@@ -114,6 +114,7 @@ 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,
@@ -181,6 +182,7 @@ class Navigation {
   std::vector<ObstacleCost> GetGlobalCostmapObstacles();
 
   Eigen::Vector2f GetIntermediateGoal();
+  bool verbose_logging;
 
  private: