armem::timestamp instead of long

source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp

@@ -31,6 +31,7 @@
 
 // Header
 #include "ArmarXCore/core/logging/Logging.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
 #include "RobotAPI/libraries/aron/core/navigator/data/primitive/Primitive.h"
 #include "RobotAPI/libraries/core/Pose.h"
 
@@ -347,6 +348,9 @@ namespace armarx::armem::server::robot_state
                     odometryPose.translation() << relPosX, relPosY, 0; // TODO set height
                     odometryPose.linear() = simox::math::rpy_to_mat3f(0.F, 0.F, relOrientation);
 
+                    // const auto timestamp = armem::Time::microSeconds(it->second.timestamp);
+                    const auto timestamp = armem::Time::now();
+
                     const ::armarx::armem::robot_state::Transform transform
                     {
                         .header =
@@ -354,7 +358,7 @@ namespace armarx::armem::server::robot_state
                             .parentFrame = OdometryFrame,
                             .frame = "root", // TODO: robot root node
                             .agent = robotUnitProviderID.providerSegmentName,
-                            .timestamp = it->second.timestamp
+                            .timestamp = timestamp
                         },
                         .transform = odometryPose
                     };

source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h

@@ -13,6 +13,7 @@
 #include <RobotAPI/libraries/armem/core/ice_conversions.h>
 
 #include "BaseQueryProcessorBase.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
 
 
 namespace armarx::armem::base::query_proc
@@ -252,6 +253,9 @@ namespace armarx::armem::base::query_proc
             // if 'before' is already invalid, there is nothing to be gained here.
             if (not isBeforeOrAtValid)
             {
+                const armem::Time dt = referenceTimestamp - entity.getLatestTimestamp();
+
+                ARMARX_WARNING << "Lookup " << dt.toMilliSeconds() << " ms into the future.";
                 return;
             }
 
@@ -271,10 +275,19 @@ namespace armarx::armem::base::query_proc
             {
                 addResultSnapshot(result, beforeOrAt);
             }
+            else
+            {
+                ARMARX_WARNING << "No valid entity before timestamp";
+            }
+
             if (isInRange(after->first))
             {
                 addResultSnapshot(result, after);
             }
+            else
+            {
+                ARMARX_WARNING << "No valid entity after timestamp";
+            }
         }
 
 

source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp

@@ -38,7 +38,7 @@ namespace armarx::armem
         aron::toAron(dto.parentFrame, bo.parentFrame);
         aron::toAron(dto.frame, bo.frame);
         aron::toAron(dto.agent, bo.agent);
-        aron::toAron(dto.timestamp, bo.timestamp);
+        armarx::toAron(dto.timestamp, bo.timestamp);
     }
 
     void fromAron(const arondto::TransformHeader& dto, robot_state::TransformHeader& bo)
@@ -46,7 +46,7 @@ namespace armarx::armem
         aron::fromAron(dto.parentFrame, bo.parentFrame);
         aron::fromAron(dto.frame, bo.frame);
         aron::fromAron(dto.agent, bo.agent);
-        aron::fromAron(dto.timestamp, bo.timestamp);
+        armarx::fromAron(dto.timestamp, bo.timestamp);
     }
 
     /* JointState */

source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp

@@ -21,8 +21,8 @@
  */
 
 #include "TransformReader.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
 
-#include <Eigen/src/Geometry/Transform.h>
 #include <algorithm>
 #include <iterator>
 #include <numeric>
@@ -98,7 +98,7 @@ namespace armarx::armem::client::robot_state::localization
 
     TransformResult TransformReader::getGlobalPose(const std::string& agentName,
             const std::string& robotRootFrame,
-            const std::int64_t& timestamp) const
+            const armem::Time& timestamp) const
     {
         const TransformQuery query{.header = {.parentFrame = GlobalFrame,
                                               .frame       = robotRootFrame,
@@ -133,7 +133,7 @@ namespace armarx::armem::client::robot_state::localization
 
     TransformResult TransformReader::lookupTransform(const TransformQuery& query) const
     {
-        const auto timestamp = IceUtil::Time::microSeconds(query.header.timestamp);
+        const auto& timestamp = query.header.timestamp;
 
         const auto durationEpsilon = IceUtil::Time::milliSeconds(100);
 
@@ -147,7 +147,7 @@ namespace armarx::armem::client::robot_state::localization
         .coreSegments().withName(properties.localizationSegment)
         .providerSegments().withName(query.header.agent) // agent
         .entities().all() // parentFrame,frame
-        .snapshots().latest(); //timeRange(timestamp - durationEpsilon, timestamp + durationEpsilon);
+        .snapshots().atTimeApprox(timestamp - durationEpsilon, IceUtil::Time::microSeconds(-1)); //timeRange(timestamp - durationEpsilon, timestamp + durationEpsilon);
         // clang-format on
 
         // TODO(fabian.reister): remove latest() and add atTime

source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h

@@ -53,7 +53,7 @@ namespace armarx::armem::client::robot_state::localization
 
         TransformResult getGlobalPose(const std::string& agentName,
                                       const std::string& robotRootFrame,
-                                      const std::int64_t& timestamp) const override;
+                                      const armem::Time& timestamp) const override;
 
         TransformResult lookupTransform(const TransformQuery& query) const override;
 

source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp

@@ -118,7 +118,9 @@ namespace armarx::armem::client::robot_state::localization
             return false;
         }
 
-        const auto timestamp = IceUtil::Time::microSeconds(transform.header.timestamp);
+        // const auto& timestamp = transform.header.timestamp;
+        const auto timestamp = IceUtil::Time::now(); // FIXME remove
+
 
         const auto entityID   = providerId->withEntityName(transform.header.parentFrame + "," +
                                 transform.header.frame).withTimestamp(timestamp);
@@ -130,9 +132,9 @@ namespace armarx::armem::client::robot_state::localization
         toAron(aronTransform, transform);
 
         update.instancesData = {aronTransform.toAron()};
-        update.timeCreated = IceUtil::Time::microSeconds(aronTransform.header.timestamp);
+        update.timeCreated = timestamp;
 
-        ARMARX_DEBUG << "Committing " << update << " at time " << IceUtil::Time::microSeconds(transform.header.timestamp);
+        ARMARX_DEBUG << "Committing " << update << " at time " << transform.header.timestamp;
         armem::EntityUpdateResult updateResult = memoryWriter.commit(update);
 
         ARMARX_DEBUG << updateResult;

source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h

@@ -51,7 +51,7 @@ namespace armarx::armem::client::robot_state::localization
 
         virtual TransformResult getGlobalPose(const std::string& agentName,
                                               const std::string& robotRootFrame,
-                                              const std::int64_t& timestamp) const = 0;
+                                              const armem::Time& timestamp) const = 0;
 
         virtual TransformResult lookupTransform(const TransformQuery& query) const = 0;
         // waitForTransform()

source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp

@@ -5,6 +5,7 @@
 #include <SimoxUtility/color/cmaps.h>
 #include <SimoxUtility/math/pose/interpolate.h>
 
+#include "ArmarXCore/core/exceptions/LocalException.h"
 #include "RobotAPI/libraries/core/FramedPose.h"
 
 #include "RobotAPI/libraries/aron/common/aron_conversions.h"
@@ -147,9 +148,9 @@ namespace armarx::armem::common::robot_state::localization
 
     auto
     findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
-                          const int64_t timestamp)
+                          const armem::Time& timestamp)
     {
-        const auto comp = [](const int64_t& timestamp, const auto & transform)
+        const auto comp = [](const armem::Time & timestamp, const auto & transform)
         {
             return transform.header.timestamp < timestamp;
         };
@@ -173,7 +174,7 @@ namespace armarx::armem::common::robot_state::localization
 
     Eigen::Affine3f
     interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform>& queue,
-                         int64_t timestamp)
+                         armem::Time timestamp)
     {
         ARMARX_TRACE;
 
@@ -208,19 +209,19 @@ namespace armarx::armem::common::robot_state::localization
         ARMARX_DEBUG << "deref";
 
         // the time fraction [0..1] of the lookup wrt to posePre and poseNext
-        const float t = static_cast<float>(timestamp - posePreIt->header.timestamp) /
-                        (poseNextIt->header.timestamp - posePreIt->header.timestamp);
+        const double t = (timestamp - posePreIt->header.timestamp).toMicroSecondsDouble() /
+                         (poseNextIt->header.timestamp - posePreIt->header.timestamp).toMicroSecondsDouble();
 
         ARMARX_DEBUG << "interpolate";
 
-        return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, t);
+        return simox::math::interpolatePose(posePreIt->transform, poseNextIt->transform, static_cast<float>(t));
     }
 
     std::vector<Eigen::Affine3f>
     TransformHelper::obtainTransforms(const armem::wm::CoreSegment& localizationCoreSegment,
                                       const std::vector<std::string>& tfChain,
                                       const std::string& agent,
-                                      const std::int64_t& timestamp)
+                                      const armem::Time& timestamp)
     {
         const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(agent);
 
@@ -251,8 +252,12 @@ namespace armarx::armem::common::robot_state::localization
         {
             ARMARX_WARNING << "local exception: " << ex.what();
         }
+        catch (...)
+        {
+            ARMARX_WARNING << "Error: " << GetHandledExceptionString();
+        }
+
 
-        ARMARX_WARNING << "Unkown error.";
 
         return {};
     }
@@ -260,7 +265,7 @@ namespace armarx::armem::common::robot_state::localization
     Eigen::Affine3f
     TransformHelper::obtainTransform(const std::string& entityName,
                                      const armem::wm::ProviderSegment& agentProviderSegment,
-                                     const int64_t timestamp)
+                                     const armem::Time& timestamp)
     {
 
         ARMARX_DEBUG << "getEntity:" + entityName;

source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h

@@ -41,25 +41,25 @@ namespace armarx::armem::common::robot_state::localization
 
     class TransformHelper
     {
-      public:
+    public:
         static TransformResult
-        lookupTransform(const armem::wm::CoreSegment &localizationCoreSegment,
-                        const TransformQuery &query);
+        lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+                        const TransformQuery& query);
 
-      private:
+    private:
         static std::vector<std::string>
-        buildTransformChain(const armem::wm::CoreSegment &localizationCoreSegment,
-                            const TransformQuery &query);
+        buildTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+                            const TransformQuery& query);
 
         static std::vector<Eigen::Affine3f>
-        obtainTransforms(const armem::wm::CoreSegment &localizationCoreSegment,
-                         const std::vector<std::string> &tfChain,
-                         const std::string &agent,
-                         const std::int64_t &timestamp);
+        obtainTransforms(const armem::wm::CoreSegment& localizationCoreSegment,
+                         const std::vector<std::string>& tfChain,
+                         const std::string& agent,
+                         const armem::Time& timestamp);
 
         static Eigen::Affine3f
-        obtainTransform(const std::string &entityName,
-                        const armem::wm::ProviderSegment &agentProviderSegment,
-                        int64_t timestamp);
+        obtainTransform(const std::string& entityName,
+                        const armem::wm::ProviderSegment& agentProviderSegment,
+                        const armem::Time& timestamp);
     };
 } // namespace armarx::armem::common::robot_state::localization
\ No newline at end of file

source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp

@@ -89,7 +89,7 @@ namespace armarx::armem::server::robot_state::localization
                     .parentFrame = GlobalFrame,
                     .frame       = "root", // TODO, FIXME
                     .agent       = robotName,
-                    .timestamp   = timestamp.toMicroSeconds()
+                    .timestamp   = timestamp
                 }};
 
             const auto result =
@@ -112,7 +112,7 @@ namespace armarx::armem::server::robot_state::localization
 
     bool Segment::storeTransform(const armarx::armem::robot_state::Transform& transform)
     {
-        const auto timestamp = IceUtil::Time::microSeconds(transform.header.timestamp);
+        const auto& timestamp = transform.header.timestamp;
 
         const MemoryID providerID =
             coreSegment->id().withProviderSegmentName(transform.header.agent);

source/RobotAPI/libraries/armem_robot_state/types.h

+/*
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date       2021
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
 #pragma once
 
 #include <Eigen/Geometry>
 
+#include "RobotAPI/libraries/armem/core/Time.h"
+
 namespace armarx::armem::robot_state
 {
     struct TransformHeader
@@ -11,7 +34,7 @@ namespace armarx::armem::robot_state
 
         std::string agent;
 
-        std::int64_t timestamp; // [µs]
+        armem::Time timestamp; 
     };
 
     struct Transform

source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp

@@ -21,5 +21,14 @@ namespace armarx
         dto.path                       = icedto.path;
     }
 
+    void fromAron(const long& dto, IceUtil::Time& bo)
+    {
+        bo = IceUtil::Time::microSeconds(dto);
+    }
+
+    void toAron(long& dto, const IceUtil::Time& bo)
+    {
+        dto = bo.toMicroSeconds();
+    }
 
 } // namespace armarx

source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h

@@ -11,4 +11,7 @@ namespace armarx
     void fromAron(const arondto::PackagePath& dto, PackagePath& bo);
     void toAron(arondto::PackagePath& dto, const PackagePath& bo);
 
+    void fromAron(const long& dto, IceUtil::Time& bo);
+    void toAron(long& dto, const IceUtil::Time& bo);
+
 }  // namespace armarx