From 87938bd81728a890f6a92d3c2400f1ffa6b2b9da Mon Sep 17 00:00:00 2001
From: Fabian Paus <fabian.paus@kit.edu>
Date: Sun, 17 Nov 2019 18:12:28 +0100
Subject: [PATCH] ArViz: Get timestamps for replay and display them
---
.../ArViz/ArVizWidgetController.cpp | 34 ++++++++++++++-----
.../gui-plugins/ArViz/ArVizWidgetController.h | 2 +-
2 files changed, 26 insertions(+), 10 deletions(-)
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 6b5c54779..88dc0f434 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -346,7 +346,6 @@ void ArVizWidgetController::onRefreshRecordings()
currentText = currentItem->text().toStdString();
}
- // TODO: Remember old selected entry and try to restore
widget.recordingList->clear();
int currentTextIndex = -1;
int index = 0;
@@ -384,11 +383,15 @@ void ArVizWidgetController::onRecordingSelectionChanged(QListWidgetItem* current
}
}
-static std::string timestampToString(long timestampInMicroSeconds)
+static std::string timestampToString(long timestampInMicroSeconds, bool showMS = false)
{
IceUtil::Time time = IceUtil::Time::microSeconds(timestampInMicroSeconds);
std::string timeString = time.toDateTime();
- return timeString.substr(0, timeString.size() - 4);
+ if (!showMS)
+ {
+ timeString = timeString.substr(0, timeString.size() - 4);
+ };
+ return timeString;
}
void ArVizWidgetController::onReplaySpinChanged(int newValue)
@@ -403,13 +406,11 @@ void ArVizWidgetController::onReplaySliderChanged(int newValue)
currentRevision = newValue;
widget.replayRevisionSpinBox->setValue(newValue);
- // TODO: Get the correct timestamp from the batch
- long timestamp = currentRecording.firstTimestampInMicroSeconds;
+ long timestamp = replayToRevision(currentRevision);
widget.replayTimestampLabel->setText(QString::fromStdString(
- timestampToString(timestamp)
+ timestampToString(timestamp, true)
));
- replayToRevision(currentRevision);
}
}
@@ -457,7 +458,7 @@ void ArVizWidgetController::selectRecording(const viz::Recording& recording)
}
-void ArVizWidgetController::replayToRevision(long revision)
+long ArVizWidgetController::replayToRevision(long revision)
{
viz::RecordingBatchHeader* matchingBatchHeader = nullptr;
for (auto& batchHeader : currentRecording.batchHeaders)
@@ -472,7 +473,7 @@ void ArVizWidgetController::replayToRevision(long revision)
if (matchingBatchHeader == nullptr)
{
ARMARX_WARNING << "Could not find batch for revision: " << revision;
- return;
+ return -1;
}
viz::RecordingBatch const& batch = getRecordingBatch(matchingBatchHeader->index);
@@ -481,6 +482,21 @@ void ArVizWidgetController::replayToRevision(long revision)
<< "\nGot batch: " << batch.header.firstRevision << " - " << batch.header.lastRevision
<< "\nUpdates: " << batch.updates.size()
<< "\nInitial state: " << batch.initialState.size();
+
+
+ auto revisionLess = [](viz::TimestampedLayerUpdate const & lhs, viz::TimestampedLayerUpdate const & rhs)
+ {
+ return lhs.revision < rhs.revision;
+ };
+ viz::TimestampedLayerUpdate pivot;
+ pivot.revision = revision;
+ auto updateBegin = std::lower_bound(batch.updates.begin(), batch.updates.end(), pivot, revisionLess);
+ auto updateEnd = std::upper_bound(updateBegin, batch.updates.end(), pivot, revisionLess);
+
+ // TODO: Set current state to batch initial state
+ // Apply updates until updateEnd
+
+ return updateBegin->timestampInMicroseconds;
}
viz::RecordingBatch const& ArVizWidgetController::getRecordingBatch(long index)
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index ce720de88..822104b64 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -132,7 +132,7 @@ namespace armarx
void selectRecording(viz::Recording const& recording);
- void replayToRevision(long revision);
+ long replayToRevision(long revision);
private:
Ui::ArVizWidget widget;
--
GitLab