added mutex for robotstatehistory

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)
         {

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;