Added a timing parameter

Author: sdudiak-husarion

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -122,6 +122,7 @@ class MPPIController : public nav2_core::Controller
   TrajectoryVisualizer trajectory_visualizer_;
 
   bool visualize_;
+  bool print_execution_times_;
 };
 
 }  // namespace nav2_mppi_controller

nav2_mppi_controller/src/controller.cpp

@@ -37,6 +37,7 @@ void MPPIController::configure(
   // Get high-level controller parameters
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(visualize_, "visualize", false);
+  getParam(print_execution_times_,"print_execution_times", false);
 
   // Configure composed objects
   optimizer_.initialize(parent_, name_, costmap_ros_, parameters_handler_.get());
@@ -79,9 +80,11 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   const geometry_msgs::msg::Twist & robot_speed,
   nav2_core::GoalChecker * goal_checker)
 {
-#ifdef BENCHMARK_TESTING
-  auto start = std::chrono::system_clock::now();
-#endif
+  std::chrono::time_point<std::chrono::system_clock> start;
+  if(print_execution_times_)
+  {
+    start = std::chrono::system_clock::now();
+  }
 
   std::lock_guard<std::mutex> param_lock(*parameters_handler_->getLock());
   geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose;
@@ -94,11 +97,13 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   geometry_msgs::msg::TwistStamped cmd =
     optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker);
 
-#ifdef BENCHMARK_TESTING
-  auto end = std::chrono::system_clock::now();
-  auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
-  RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration);
-#endif
+  if (print_execution_times_)
+  {
+    auto end = std::chrono::system_clock::now();
+    auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
+    RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration);
+  }
+
 
   if (visualize_) {
     visualize(std::move(transformed_plan), cmd.header.stamp);