diff --git a/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp b/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp index 2c6d98938b7d20b177764afddbedb2f6f8286804..46bd21639bead4a6dc1f498b693b379ea2e6f10e 100644 --- a/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp +++ b/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); }