Merge branch 'RobotStateHistory' of https://gitlab.com/ArmarX/RobotAPI into RobotStateHistory

source/RobotAPI/components/RobotState/test/RobotStateTest.cpp

@@ -43,9 +43,9 @@ public:
         CMakePackageFinder finder("RobotAPI");
         ArmarXDataPath::addDataPaths({finder.getDataDir()});
         // If you need to set properties (optional):
-         properties->setProperty("ArmarX.RobotStateComponent.AgentName", "Armar3");
-         properties->setProperty("ArmarX.RobotStateComponent.RobotNodeSetName", "Robot");
-         properties->setProperty("ArmarX.RobotStateComponent.RobotFileName", "RobotAPI/robots/Armar3/ArmarIII.xml");
+        properties->setProperty("ArmarX.RobotStateComponent.AgentName", "Armar3");
+        properties->setProperty("ArmarX.RobotStateComponent.RobotNodeSetName", "Robot");
+        properties->setProperty("ArmarX.RobotStateComponent.RobotFileName", "RobotAPI/robots/Armar3/ArmarIII.xml");
 
         // The IceTestHelper starts all required Ice processes
         _iceTestHelper = new IceTestHelper(registryPort, registryPort + 1);
@@ -76,28 +76,28 @@ BOOST_AUTO_TEST_CASE(RobotStateComponentHistoryTest)
     RobotStateComponentTestEnvironment env("RSC");
 
     NameValueMap joints;
-    while(env._manager->getObjectState("RobotStateComponent") < eManagedIceObjectStarted)
+    while (env._manager->getObjectState("RobotStateComponent") < eManagedIceObjectStarted)
     {
         ARMARX_INFO_S << "Waiting";
         usleep(100000);
     }
     joints["Elbow L"] = 0;
-    env._rsc->reportJointAngles(joints, true);
+    env._rsc->reportJointAngles(joints, IceUtil::Time::now().toMicroSeconds(), true);
     auto t1 = env._rsc->getSynchronizedRobot()->getTimestamp()->timestamp;
     usleep(10000);
     joints["Elbow L"] = 1;
-    env._rsc->reportJointAngles(joints, true);
+    env._rsc->reportJointAngles(joints, IceUtil::Time::now().toMicroSeconds(), true);
     auto t2 = env._rsc->getSynchronizedRobot()->getTimestamp()->timestamp;
     auto config = env._rsc->getJointConfigAtTimestamp(t1);
     BOOST_CHECK(t2 > t1);
     BOOST_CHECK_EQUAL(config["Elbow L"], 0);
     config = env._rsc->getJointConfigAtTimestamp(t2);
     BOOST_CHECK_EQUAL(config["Elbow L"], 1);
-    config = env._rsc->getJointConfigAtTimestamp(t2+1);// future timestamp -> latest values should be returned
+    config = env._rsc->getJointConfigAtTimestamp(t2 + 1); // future timestamp -> latest values should be returned
     BOOST_CHECK_EQUAL(config["Elbow L"], 1);
-    config = env._rsc->getJointConfigAtTimestamp((t1+t2)*0.5); // interpolated values in the middle
+    config = env._rsc->getJointConfigAtTimestamp((t1 + t2) * 0.5); // interpolated values in the middle
     BOOST_CHECK_CLOSE(config["Elbow L"], 0.5f, 1);
-    config = env._rsc->getJointConfigAtTimestamp(t1+(t2-t1)*0.7); // interpolated values
+    config = env._rsc->getJointConfigAtTimestamp(t1 + (t2 - t1) * 0.7); // interpolated values
     ARMARX_INFO_S << "value at t=0.7%: " << config["Elbow L"];
     BOOST_CHECK_CLOSE(config["Elbow L"], 0.7f, 1);
 }