Merge branch 'master' of gitlab.com:ArmarX/RobotAPI

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 inT1 = IceUtil::Time::seconds(1);
+    const IceUtil::Time inT2 = inT1 + IceUtil::Time::milliSeconds(110);
+
+    NameValueMap joints;
+
     joints["Elbow L"] = 0;
-    env._rsc->reportJointAngles(joints, IceUtil::Time::now().toMicroSeconds(), true);
+    env._rsc->reportJointAngles(joints, inT1.toMicroSeconds(), 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, inT2.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);
+
+    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);
 }

source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui

@@ -95,6 +95,13 @@
              </property>
             </widget>
            </item>
+           <item>
+            <widget class="QCheckBox" name="checkBoxWPReverse">
+             <property name="text">
+              <string>Reverse list</string>
+             </property>
+            </widget>
+           </item>
            <item>
             <spacer name="horizontalSpacer">
              <property name="orientation">

source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp

@@ -25,6 +25,7 @@
 
 #include <VirtualRobot/Util/json.h>
 
+#include <ArmarXCore/util/CPPUtility/container.h>
 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
 #include "CartesianWaypointControlGuiWidgetController.h"
@@ -154,7 +155,7 @@ namespace armarx
             _controller->activateController();
             ARMARX_IMPORTANT << "trigger execution of " << _lastParsedWPs.size()
                              << " waypoints on " << _controllerName;
-            _controller->setWaypoints(_lastParsedWPs);
+            _controller->setWaypoints(_ui.checkBoxWPReverse->isChecked() ? reverse(_lastParsedWPs) : _lastParsedWPs);
         }
     }