From 5007c6672efe78191f1af8c84e9f5ec8deacbedf Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 16 Oct 2018 12:13:06 +0200
Subject: [PATCH] Add param /geometric_simu/paused to pause SoT
---
src/sot_loader.cpp | 18 +++++++++++++-----
1 file changed, 13 insertions(+), 5 deletions(-)
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index f88a042..de97fe4 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -86,14 +86,22 @@ void workThreadLoader(SotLoader *aSotLoader)
}
struct timeval start, stop;
+ ros::NodeHandle nh ("/geometric_simu");
+ bool paused;
+ unsigned long long dt;
while(!aSotLoader->isDynamicGraphStopped())
{
- gettimeofday(&start,0);
- aSotLoader->oneIteration();
- gettimeofday(&stop,0);
+ nh.param<bool> ("paused", paused, false);
- unsigned long long dt = 1000000 * (stop.tv_sec - start.tv_sec) + (stop.tv_usec - start.tv_usec);
- dataToLog.record ((double)dt * 1e-6);
+ if (!paused) {
+ gettimeofday(&start,0);
+ aSotLoader->oneIteration();
+ gettimeofday(&stop,0);
+
+ dt = 1000000 * (stop.tv_sec - start.tv_sec) + (stop.tv_usec - start.tv_usec);
+ dataToLog.record ((double)dt * 1e-6);
+ } else
+ dt = 0;
if (period > dt) {
usleep(period - (unsigned)dt);
}
--
GitLab