Merge branch '13-robotstatetest-fails' into 'master'

Resolve "RobotStateTest fails"

Closes #13

See merge request ArmarX/RobotAPI!53

source/RobotAPI/components/RobotState/RobotStateComponent.cpp

@@ -337,7 +337,13 @@ namespace armarx
             Eigen::AngleAxisf(currentPose.rotationAroundZ, Eigen::Vector3f::UnitZ()).toRotationMatrix();
         const Eigen::Matrix4f globalPose = math::Helpers::Pose(position, orientation);
 
-        insertPose(IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds), globalPose);
+        IceUtil::Time time = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
+        insertPose(time, globalPose);
+
+        if (_sharedRobotServant)
+        {
+            _sharedRobotServant->setTimestamp(time);
+        }
     }
 
     void RobotStateComponent::reportNewTargetPose(
@@ -521,7 +527,7 @@ namespace armarx
             return jointHistory.rbegin()->second;
         }
 
-        // Get the next newer entry than time.
+        // Get the oldest entry whose time >= time.
         auto nextIt = jointHistory.lower_bound(time);
         if (nextIt == jointHistory.end())
         {
@@ -533,16 +539,17 @@ namespace armarx
             return std::nullopt;
         }
 
-
         const NameValueMap& next = nextIt->second;
-        auto prevIt = std::prev(nextIt);
 
-        if (prevIt == jointHistory.end())
+        if (nextIt == jointHistory.begin())
         {
             // Next was oldest entry.
             return next;
         }
 
+        auto prevIt = std::prev(nextIt);
+
+
         // Interpolate.
 
         IceUtil::Time prevTime = prevIt->first;
@@ -606,17 +613,17 @@ namespace armarx
         }
 
 
-        auto prevIt = std::prev(nextIt);
-        ARMARX_CHECK_EXPRESSION(prevIt->second);
-
         const FramedPosePtr& next = nextIt->second;
 
-        if (prevIt == poseHistory.end())
+        if (nextIt == poseHistory.begin())
         {
             // Next was oldest entry.
             return next;
         }
 
+        auto prevIt = std::prev(nextIt);
+
+        ARMARX_CHECK_EXPRESSION(prevIt->second);
         const FramedPosePtr& prev = prevIt->second;
 
 

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);
 }