From 5bbc8f14cb3584a5e3912121ab8e6989f08b3280 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 14 May 2021 13:12:36 +0200
Subject: [PATCH] armem::timestamp instead of long
---
.../RobotStateMemory/RobotStateMemory.cpp | 6 ++++-
.../base/EntityQueryProcessorBase.h | 13 ++++++++++
.../armem_robot_state/aron_conversions.cpp | 4 +--
.../client/localization/TransformReader.cpp | 8 +++---
.../client/localization/TransformReader.h | 2 +-
.../client/localization/TransformWriter.cpp | 8 +++---
.../client/localization/interfaces.h | 2 +-
.../common/localization/TransformHelper.cpp | 23 +++++++++-------
.../common/localization/TransformHelper.h | 26 +++++++++----------
.../server/localization/Segment.cpp | 4 +--
.../libraries/armem_robot_state/types.h | 25 +++++++++++++++++-
.../aron/common/aron_conversions/armarx.cpp | 9 +++++++
.../aron/common/aron_conversions/armarx.h | 3 +++
13 files changed, 96 insertions(+), 37 deletions(-)
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 2e49c6261..444c6fdc3 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
+++ b/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
};
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
index 9b8584779..dcc1fbf78 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/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";
+ }
}
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index dc30cbeb4..27fecee2e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
+++ b/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 */
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
index 1c9289135..e7b55ef6f 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/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
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
index 53d3ad522..fc789a7d9 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.h
+++ b/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;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
index 8521c583e..8755cee13 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformWriter.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h b/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
index e8afaf3e4..ae35822c6 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/interfaces.h
+++ b/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()
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
index ec936fb23..c8db83835 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/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;
diff --git a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
index 7505dd385..8045c552e 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.h
+++ b/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 ×tamp);
+ 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
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
index 118f50b56..6858f0c34 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/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);
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
index 43408df40..39daba5d2 100644
--- a/source/RobotAPI/libraries/armem_robot_state/types.h
+++ b/source/RobotAPI/libraries/armem_robot_state/types.h
@@ -1,7 +1,30 @@
+/*
+ * 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
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
index ea25e01e3..2cd6b7eb9 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.cpp
+++ b/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
diff --git a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h b/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
index 985409745..48c246905 100644
--- a/source/RobotAPI/libraries/aron/common/aron_conversions/armarx.h
+++ b/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
--
GitLab