RobotStateComponent: Fixed segfault in reportJointAngles

source/RobotAPI/components/RobotState/RobotStateComponent.cpp

@@ -253,7 +253,7 @@ namespace armarx
                     float influenceNext = 1.0f - (float)(it->first - time) / deltaT;
                     float influencePrev = 1.0f - (float)(time - prevIt->first) / deltaT;
                     auto jointIt = prevIt->second.jointMap.begin();
-                    for (auto & joint : config.jointMap)
+                    for (auto& joint : config.jointMap)
                     {
                         joint.second = joint.second * influenceNext + jointIt->second * influencePrev;
                         jointIt++;
@@ -279,11 +279,11 @@ namespace armarx
 
     void RobotStateComponent::reportJointAngles(const NameValueMap& jointAngles, Ice::Long timestamp, bool aValueChanged, const Current& c)
     {
-        //        IceUtil::Time start = IceUtil::Time::now();
         if (timestamp <= 0)
         {
             timestamp = IceUtil::Time::now().toMicroSeconds();
         }
+
         if (aValueChanged)
         {
             {
@@ -311,18 +311,16 @@ namespace armarx
             _sharedRobotServant->setTimestamp(IceUtil::Time::microSeconds(timestamp));
         }
 
-        history[timestamp] = {timestamp,
-                              new FramedPose(_synchronized->getGlobalPose(), GlobalFrame, ""),
-                              jointAngles
-                             };
+        RobotStateConfig config;
+        config.timestamp = timestamp;
+        config.globalPose = new FramedPose(_synchronized->getGlobalPose(), GlobalFrame, "");
+        config.jointMap = jointAngles;
+        history[timestamp] = config;
 
         if (history.size() > historyLength)
         {
             history.erase(history.begin());
         }
-        //        ARMARX_VERBOSE << deactivateSpam(2) << "my duration: " << (IceUtil::Time::now() - start).toMicroSecondsDouble() << " ms";
-
-
     }
 
     std::string RobotStateComponent::getRobotFilename(const Ice::Current&) const
@@ -341,7 +339,7 @@ namespace armarx
         auto packages = armarx::Application::GetProjectDependencies();
         packages.push_back(Application::GetProjectName());
 
-        for (const std::string & projectName : packages)
+        for (const std::string& projectName : packages)
         {
             if (projectName.empty())
             {