Resolve "RobotStateTest fails"

Closes #13 (closed)

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

@@ -75,29 +75,42 @@ BOOST_AUTO_TEST_CASE(RobotStateComponentHistoryTest)
 {
     RobotStateComponentTestEnvironment env("RSC");
 
-    NameValueMap joints;
     while (env._manager->getObjectState("RobotStateComponent") < eManagedIceObjectStarted)
     {
         ARMARX_INFO_S << "Waiting";
         usleep(100000);
     }
+
+    const IceUtil::Time t1In = IceUtil::Time::seconds(1);
+    const IceUtil::Time t2In = t1In + IceUtil::Time::milliSeconds(110);
+
+    NameValueMap joints;
+
     joints["Elbow L"] = 0;
-    env._rsc->reportJointAngles(joints, IceUtil::Time::now().toMicroSeconds(), true);
+    env._rsc->reportJointAngles(joints, t1In, true);
     auto t1 = env._rsc->getSynchronizedRobot()->getTimestamp()->timestamp;
-    usleep(10000);
+
     joints["Elbow L"] = 1;
-    env._rsc->reportJointAngles(joints, IceUtil::Time::now().toMicroSeconds(), true);
+    env._rsc->reportJointAngles(joints, t2In, 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);
+
+    auto config = env._rsc->getJointConfigAtTimestamp(t1);
+
+    BOOST_REQUIRE_EQUAL(config.count("Elbow L"), 1);
+
+    BOOST_CHECK_EQUAL(config.at("Elbow L"), 0);
+
     config = env._rsc->getJointConfigAtTimestamp(t2);
-    BOOST_CHECK_EQUAL(config["Elbow L"], 1);
+    BOOST_CHECK_EQUAL(config.at("Elbow L"), 1);
+
     config = env._rsc->getJointConfigAtTimestamp(t2 + 1); // future timestamp -> latest values should be returned
-    BOOST_CHECK_EQUAL(config["Elbow L"], 1);
+    BOOST_CHECK_EQUAL(config.at("Elbow L"), 1);
+
     config = env._rsc->getJointConfigAtTimestamp((t1 + t2) * 0.5); // interpolated values in the middle
-    BOOST_CHECK_CLOSE(config["Elbow L"], 0.5f, 0.01);
+    BOOST_CHECK_CLOSE(config.at("Elbow L"), 0.5f, 0.01);
+
     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, 0.01);
+    ARMARX_INFO_S << "value at t=0.7%: " << config.at("Elbow L");
+    BOOST_CHECK_CLOSE(config.at("Elbow L"), 0.7f, 0.01);
 }