From 185c2ffb281436ac5975e22ad61824e9ad7d5d7c Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Wed, 30 Mar 2016 12:53:27 +0200
Subject: [PATCH] added mutex for robotstatehistory
---
.../RobotState/RobotStateComponent.cpp | 20 +++++++++++--------
.../RobotState/RobotStateComponent.h | 2 ++
2 files changed, 14 insertions(+), 8 deletions(-)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index cedadbba4..17c7724ca 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -230,8 +230,12 @@ namespace armarx
bool RobotStateComponent::interpolate(double time, RobotStateConfig& config) const
{
-
+ boost::shared_lock<SharedMutex> lock(historyMutex);
auto it = history.lower_bound(time);
+ if (history.size() == 0)
+ {
+ return false;
+ }
if (time > history.rbegin()->first)
{
config = history.rbegin()->second;
@@ -245,7 +249,7 @@ namespace armarx
config = it->second;
if (it->first == time)
{
-// ARMARX_INFO_S << "No Interpolation needed: " << time;
+ // ARMARX_INFO_S << "No Interpolation needed: " << time;
}
else
{
@@ -256,7 +260,7 @@ namespace armarx
long deltaT = it->first - prevIt->first;
double influenceNext = 1.0f - (double)(it->first - time) / deltaT;
double influencePrev = 1.0f - (double)(time - prevIt->first) / deltaT;
-// ARMARX_INFO_S << "Interpolating: " << time << " prev: " << time - prevIt->first << " next: " << it->first - time << VAROUT(influenceNext) << VAROUT(influencePrev) << " sum: " << (influenceNext + influencePrev);
+ // ARMARX_INFO_S << "Interpolating: " << time << " prev: " << time - prevIt->first << " next: " << it->first - time << VAROUT(influenceNext) << VAROUT(influencePrev) << " sum: " << (influenceNext + influencePrev);
auto jointIt = prevIt->second.jointMap.begin();
for (auto & joint : config.jointMap)
{
@@ -315,11 +319,11 @@ namespace armarx
{
_sharedRobotServant->setTimestamp(time);
}
-
- history[time.toMicroSecondsDouble()] = {time.toMicroSecondsDouble(),
- new FramedPose(_synchronized->getGlobalPose(), GlobalFrame, ""),
- jointAngles
- };
+ boost::unique_lock<SharedMutex> lock(historyMutex);
+ history.insert(history.end(), std::make_pair(time.toMicroSecondsDouble(), RobotStateConfig {time.toMicroSecondsDouble(),
+ new FramedPose(_synchronized->getGlobalPose(), GlobalFrame, ""),
+ jointAngles
+ }));
if (history.size() > historyLength)
{
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 828930463..781da9a4d 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -171,6 +171,8 @@ namespace armarx
std::string robotFile;
std::string relativeRobotFile;
RobotStateObserverPtr robotStateObs;
+
+ mutable SharedMutex historyMutex;
std::map<double, RobotStateConfig> history;
size_t historyLength;
--
GitLab