From e5067276b3a61de9056d0ff0fe4d7322e4814e6d Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Thu, 31 Oct 2024 11:18:19 -0500 Subject: [PATCH] Update online example to pause every 10 seconds --- .../src/online_planning_example.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/tesseract_examples/src/online_planning_example.cpp b/tesseract_examples/src/online_planning_example.cpp index 7baeb9f27b..05378609df 100644 --- a/tesseract_examples/src/online_planning_example.cpp +++ b/tesseract_examples/src/online_planning_example.cpp @@ -322,11 +322,12 @@ bool OnlinePlanningExample::onlinePlan() console_bridge::setLogLevel(console_bridge::LogLevel::CONSOLE_BRIDGE_LOG_INFO); using namespace std::chrono; - auto prev_start = high_resolution_clock::now(); + auto user_input_start = steady_clock::now(); + auto prev_start = steady_clock::now(); while (realtime_running_) { - auto start = high_resolution_clock::now(); - auto dt = 0.01 * duration(start - prev_start).count(); + auto start = steady_clock::now(); + auto dt = 0.01 * std::chrono::duration(start - prev_start).count(); prev_start = start; // Calculate current position of the robot and update environment current state @@ -383,12 +384,19 @@ bool OnlinePlanningExample::onlinePlan() solver.setBoxSize(box_size_); } - auto stop = high_resolution_clock::now(); + auto stop = steady_clock::now(); auto duration = duration_cast(stop - start) / static_cast(num_steps); // Update manipulator joint values and plot trajectory updateAndPlotTrajectory(x); + // Wait for user input every 5 seconds to allow viewing the trajectory + if (std::chrono::duration(steady_clock::now() - user_input_start).count() > 10) + { + plotter_->waitForInput("Pausing to allow viewing of trajectory. Hit enter to continue online planner."); + user_input_start = steady_clock::now(); + } + std::string message = "Solver Frequency (Hz): " + std::to_string(1.0 / static_cast(duration.count()) * 1000000.) + "\nCost: " + std::to_string(nlp_->evaluateTotalExactCost(x));