diff --git a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
index 338bdc68adb2918c40707048664c773447404fa4..837fcbcf8278e12b120b3dc9ff0a0b1de514fc9a 100644
--- a/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
+++ b/source/RobotAPI/components/armem/server/ObjectMemory/ObjectMemory.cpp
@@ -84,60 +84,43 @@ namespace armarx::armem::server::obj
instance::SegmentAdapter::init();
- // class segment
- try
+ const auto initSegmentWithCatch = [&](const std::string & segmentName, const auto&& fn)
{
- classSegment.init();
- }
- catch (const LocalException& e)
- {
- ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
- }
- catch (const std::exception& e)
- {
- ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
- }
- catch (...)
+ try
+ {
+ fn();
+ }
+ catch (const LocalException& e)
+ {
+ ARMARX_ERROR << "Failed to init " << segmentName << " segment. Reason: \n" << e.what();
+ }
+ catch (const std::exception& e)
+ {
+ ARMARX_ERROR << "Failed to init " << segmentName << " segment. Reason: \n" << e.what();
+ }
+ catch (...)
+ {
+ ARMARX_ERROR << "Failed to init " << segmentName << " segment for unknown reason.";
+ }
+ };
+
+ initSegmentWithCatch("class", [&]()
{
- ARMARX_ERROR << "Failed to init class segment for unknown reason.";
- }
+ classSegment.init();
+ });
- // articulated object class segment
- try
+ initSegmentWithCatch("articulated object class", [&]()
{
articulatedObjectClassSegment.init();
- }
- catch (const LocalException& e)
- {
- ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
- }
- catch (const std::exception& e)
- {
- ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
- }
- catch (...)
- {
- ARMARX_ERROR << "Failed to init class segment for unknown reason.";
- }
+ });
- // articulated object instance segment
- try
+ initSegmentWithCatch("articulated object instance", [&]()
{
articulatedObjectInstanceSegment.setArticulatedObjectClassSegment(articulatedObjectClassSegment);
articulatedObjectInstanceSegment.init();
- }
- catch (const LocalException& e)
- {
- ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
- }
- catch (const std::exception& e)
- {
- ARMARX_ERROR << "Failed to init class segment. Reason: \n" << e.what();
- }
- catch (...)
- {
- ARMARX_ERROR << "Failed to init class segment for unknown reason.";
- }
+ });
+
+
}
void ObjectMemory::onConnectComponent()
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.cpp
index 95c726f646706a3649d84d7c87bad16b6346cd8c..5aba1210686f0cc0a821d83f1053e65a41c348e6 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"
@@ -72,11 +73,13 @@ namespace armarx::armem::server::robot_state
armarx::PropertyDefinitionsPtr RobotStateMemory::createPropertyDefinitions()
{
+ const std::string robotUnitPrefix{sensorValuePrefix};
+
armarx::PropertyDefinitionsPtr defs = new ComponentPropertyDefinitions(getConfigIdentifier());
- defs->optional(robotUnitSensorPrefix, "RobotUnit.SensorValuePrefix", "Prefix of all sensor values");
- defs->optional(robotUnitMemoryBatchSize, "RobotUnit.MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
- defs->optional(robotUnitPollFrequency, "RobotUnit.UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
- defs->optional(robotUnitConfigPath, "robotUnit.ConfigPath", "Specify a configuration file to group the sensor values specifically.");
+ defs->optional(robotUnitSensorPrefix, robotUnitPrefix + "SensorValuePrefix", "Prefix of all sensor values");
+ defs->optional(robotUnitMemoryBatchSize, robotUnitPrefix + "MemoryBatchSize", "The size of the entity snapshot to send to the memory. Min is 1");
+ defs->optional(robotUnitPollFrequency, robotUnitPrefix + "UpdateFrequency", "The frequency in Hz to store values. All other values get discarded. Min is 1, max is " + std::to_string(ROBOT_UNIT_MAXIMUM_FREQUENCY));
+ defs->optional(robotUnitConfigPath, robotUnitPrefix + "ConfigPath", "Specify a configuration file to group the sensor values specifically.");
descriptionSegment.defineProperties(defs);
proprioceptionSegment.defineProperties(defs);
@@ -307,8 +310,7 @@ namespace armarx::armem::server::robot_state
auto start = std::chrono::high_resolution_clock::now();
// send batch to memory
- armem::data::Commit c;
-
+ armem::data::Commit proprioceptionCommit;
armem::data::Commit odometryCommit;
for (unsigned int i = 0; i < robotUnitMemoryBatchSize; ++i)
@@ -325,7 +327,7 @@ namespace armarx::armem::server::robot_state
const auto& timeUSec = encTimestep.timestamp;
const auto& aron = encTimestep.data;
- armem::data::EntityUpdate& update = c.updates.emplace_back();
+ armem::data::EntityUpdate& update = proprioceptionCommit.updates.emplace_back();
update.entityID = entityID;
update.timeCreatedMicroSeconds = timeUSec;
update.instancesData = {aron->toAronDictPtr()};
@@ -346,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 =
@@ -353,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
};
@@ -363,14 +368,14 @@ namespace armarx::armem::server::robot_state
}
else
{
- ARMARX_WARNING << "no odometry data!";
+ ARMARX_INFO << deactivateSpam(1000) << "No odometry data! If you are using a mobile platform this should not have happened";
}
robotUnitDataQueue.pop();
}
- auto results = commit(c);
+ auto results = commit(proprioceptionCommit);
auto stop = std::chrono::high_resolution_clock::now();
ARMARX_DEBUG << "RobotUnitData: The total runtime of sending a batch to the memory is: " << std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
index 45530f17a5adb6f0d5608123227cca9c66eea089..e8174416e9b547f70af5e5c6b578242a0234743d 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
@@ -139,6 +139,8 @@ namespace armarx::armem::server::robot_state
// params
static constexpr int ROBOT_UNIT_MAXIMUM_FREQUENCY = 100;
+ static constexpr const char* sensorValuePrefix = "RobotUnit.";
+
int robotUnitPollFrequency = 50;
std::string robotUnitSensorPrefix = "sens.*";
unsigned int robotUnitMemoryBatchSize = 50;
diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index 3833b4444736bc1f78fdf9c34d1c5d6ea4acfc8c..bfe0764272dbee12270de46cfe7cf0b90f851bb0 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/source/RobotAPI/interface/armem/query.ice
@@ -49,6 +49,57 @@ module armarx
long first = 0; ///< First index to get.
long last = -1; ///< Last index to get (inclusive).
};
+
+
+ /**
+ * @brief Get snapshot(s) around timestamp.
+ *
+ * If there is a snapshot which exactly matches the query timestamp,
+ * the behavior is as for the @see Single query.
+ *
+ * However, if there is no matching timestamp, the snapshots before
+ * and after the query timestamp will be returned.
+ *
+ * If #eps is positive, the snapshots must be within the time
+ * range [timestamp - eps, timestamp + eps].
+ * Consequently, the query result can be empty.
+ *
+ * Hint:
+ * This query can be quite useful when interpolating snapshots
+ * is possible.
+ */
+ class TimeApprox extends EntityQuery
+ {
+ long timestamp = -1;
+ long eps = -1;
+ };
+
+ /**
+ * @brief Get the last snapshot before or at the timestamp.
+ *
+ * This query is kind of similar to latest() but with a reference timestamp.
+ *
+ */
+ class BeforeOrAtTime extends EntityQuery
+ {
+ long timestamp;
+ }
+
+ /**
+ * @brief Get the last snapshot before the timestamp or a sequence of
+ * n last elements before the timestamp depending on #maxEntries
+ *
+ * Depending on #maxEntries, the behavior is as follows:
+ * - #maxEntries == 1 => last element before timestamp
+ * - #maxEntries > 1 => n last elements before timestamp
+ * - #maxEntries < 0 => all elements before timestamp
+ */
+ class BeforeTime extends EntityQuery
+ {
+ long timestamp;
+ long maxEntries = 1;
+ }
+
}
diff --git a/source/RobotAPI/libraries/armem/client/query/query_fns.h b/source/RobotAPI/libraries/armem/client/query/query_fns.h
index 8dcb88991450b95583c7228b2349fdfdf05cba95..f08ae23bc6b8b6bc7fb4ed99f9ac51ed570f556c 100644
--- a/source/RobotAPI/libraries/armem/client/query/query_fns.h
+++ b/source/RobotAPI/libraries/armem/client/query/query_fns.h
@@ -134,4 +134,34 @@ namespace armarx::armem::client::query_fns
};
}
-}
+ inline
+ std::function<void(query::SnapshotSelector&)>
+ atTimeApprox(Time time, Duration eps)
+ {
+ return [ = ](query::SnapshotSelector & selector)
+ {
+ selector.atTimeApprox(time, eps);
+ };
+ }
+
+ inline
+ std::function<void(query::SnapshotSelector&)>
+ beforeOrAtTime(Time time)
+ {
+ return [ = ](query::SnapshotSelector & selector)
+ {
+ selector.beforeOrAtTime(time);
+ };
+ }
+
+ inline
+ std::function<void(query::SnapshotSelector&)>
+ beforeTime(Time time, long nElements)
+ {
+ return [ = ](query::SnapshotSelector & selector)
+ {
+ selector.beforeTime(time, nElements);
+ };
+ }
+
+} // namespace armarx::armem::client::query_fns
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.cpp b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
index f2bae712818b566ef51444aafb84af6031d338d0..cc3a73829ab7c79eabc96c30b55c4ca350ac33ad 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.cpp
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.cpp
@@ -1,4 +1,5 @@
#include "selectors.h"
+#include "RobotAPI/libraries/armem/core/ice_conversions.h"
#include <RobotAPI/libraries/armem/core/workingmemory/ice_conversions.h>
@@ -42,6 +43,15 @@ namespace armarx::armem::client::query
return *this;
}
+ SnapshotSelector& SnapshotSelector::atTimeApprox(Time timestamp, Duration eps)
+ {
+ auto& q = _addQuery<dq::entity::TimeApprox>();
+ toIce(q.timestamp, timestamp);
+ toIce(q.eps, eps);
+ return *this;
+ }
+
+
SnapshotSelector& SnapshotSelector::indexRange(long first, long last)
{
auto& q = _addQuery<dq::entity::IndexRange>();
@@ -51,6 +61,22 @@ namespace armarx::armem::client::query
}
+ SnapshotSelector& SnapshotSelector::beforeOrAtTime(Time timestamp)
+ {
+ auto& q = _addQuery<dq::entity::BeforeOrAtTime>();
+ toIce(q.timestamp, timestamp);
+ return *this;
+ }
+
+
+ SnapshotSelector& SnapshotSelector::beforeTime(Time timestamp, long maxEntries)
+ {
+ auto& q = _addQuery<dq::entity::BeforeTime>();
+ toIce(q.timestamp, timestamp);
+ toIce(q.maxEntries, maxEntries);
+ return *this;
+ }
+
SnapshotSelector& EntitySelector::snapshots()
{
diff --git a/source/RobotAPI/libraries/armem/client/query/selectors.h b/source/RobotAPI/libraries/armem/client/query/selectors.h
index 8bc2850b390b394a5d2b5fc07753bb846f598416..a1c314232099438f9498fbabb763e52c00abe862 100644
--- a/source/RobotAPI/libraries/armem/client/query/selectors.h
+++ b/source/RobotAPI/libraries/armem/client/query/selectors.h
@@ -26,6 +26,10 @@ namespace armarx::armem::client::query
SnapshotSelector& latest();
SnapshotSelector& atTime(Time timestamp);
+ SnapshotSelector& atTimeApprox(Time timestamp, Duration eps);
+
+ SnapshotSelector& beforeTime(Time timestamp, long maxEntries);
+ SnapshotSelector& beforeOrAtTime(Time timestamp);
SnapshotSelector& timeRange(Time min, Time max);
SnapshotSelector& indexRange(long first, long last);
diff --git a/source/RobotAPI/libraries/armem/core/Time.h b/source/RobotAPI/libraries/armem/core/Time.h
index 328d1f59d793cbcc9a7d0febc0951775d200013a..f76f7657dda6e14a9c35bb4ad5edf58471680144 100644
--- a/source/RobotAPI/libraries/armem/core/Time.h
+++ b/source/RobotAPI/libraries/armem/core/Time.h
@@ -7,8 +7,8 @@
namespace armarx::armem
{
-
using Time = IceUtil::Time;
+ using Duration = IceUtil::Time;
/**
* @brief Returns `time` as e.g. "123456789.012 ms".
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 447bd259a2c24a3ca80c91c59a518daed56c18f1..dcc1fbf782a668f223c3474eb79d06446608831c 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/EntityQueryProcessorBase.h
@@ -1,15 +1,20 @@
#pragma once
-#include <RobotAPI/interface/armem/query.h>
+#include <cstdint>
+#include <iterator>
-#include "BaseQueryProcessorBase.h"
+#include <RobotAPI/interface/armem/query.h>
+#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/libraries/armem/core/error.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
+#include "BaseQueryProcessorBase.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
+
namespace armarx::armem::base::query_proc
{
@@ -48,6 +53,18 @@ namespace armarx::armem::base::query_proc
{
process(result, *q, entity);
}
+ else if (auto q = dynamic_cast<const armem::query::data::entity::TimeApprox*>(&query))
+ {
+ process(result, *q, entity);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::entity::BeforeOrAtTime*>(&query))
+ {
+ process(result, *q, entity);
+ }
+ else if (auto q = dynamic_cast<const armem::query::data::entity::BeforeTime*>(&query))
+ {
+ process(result, *q, entity);
+ }
else
{
throw armem::error::UnknownQueryType("entity snapshot", query);
@@ -152,6 +169,127 @@ namespace armarx::armem::base::query_proc
}
}
+ void process(_EntityT& result,
+ const armem::query::data::entity::BeforeOrAtTime& query,
+ const _EntityT& entity) const
+ {
+ const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+ const auto it = entity.history().lower_bound(referenceTimestamp);
+
+ if (it != entity.history().end())
+ {
+ addResultSnapshot(result, it);
+ }
+ }
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::BeforeTime& query,
+ const _EntityT& entity) const
+ {
+ const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+ const auto maxEntries = fromIce<std::int64_t>(query.maxEntries);
+
+ const auto refIt = entity.history().lower_bound(referenceTimestamp);
+
+ if (refIt == entity.history().end())
+ {
+ ARMARX_WARNING << "No valid entities found.";
+ return;
+ }
+
+ const auto nEntriesBefore = std::distance(entity.history().begin(), refIt);
+
+ const int nEntries = [&]()
+ {
+ // see query.ice
+ if (maxEntries > 0)
+ {
+ return std::min(nEntriesBefore, maxEntries);
+ }
+
+ return nEntriesBefore; // all elements before timestamp
+ }
+ ();
+
+ auto it = refIt;
+ for (std::int64_t i = 0; i < nEntries; i++, --it)
+ {
+ addResultSnapshot(result, it);
+ }
+ }
+
+ void process(_EntityT& result,
+ const armem::query::data::entity::TimeApprox& query,
+ const _EntityT& entity) const
+ {
+ const auto referenceTimestamp = fromIce<Time>(query.timestamp);
+ ARMARX_CHECK(referenceTimestamp.toMicroSeconds() >= 0) << "Reference timestamp is negative!";
+
+ const auto referenceTimestampMicroSeconds = referenceTimestamp.toMicroSeconds();
+ const auto epsDuration = fromIce<Time>(query.eps).toMicroSeconds();
+
+ // elements have to be in range [t_ref - eps, t_ref + eps] if eps is positive
+ const auto isInRange = [&](const Time & t) -> bool
+ {
+ if (epsDuration <= 0)
+ {
+ return true;
+ }
+
+ return std::abs(t.toMicroSeconds() - referenceTimestampMicroSeconds) <= epsDuration;
+ };
+
+
+ const auto beforeOrAt = entity.history().lower_bound(referenceTimestamp);
+ const auto after = entity.history().upper_bound(referenceTimestamp);
+
+ const bool isBeforeOrAtValid = beforeOrAt != entity.history().end();
+ const bool isAfterValid = isBeforeOrAtValid && after != entity.history().end();
+
+ // 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;
+ }
+
+ // only 'before' valid? or is 'before' perfect match?
+ if ((not isAfterValid) or (beforeOrAt->first == referenceTimestamp))
+ {
+ if (isInRange(beforeOrAt->first))
+ {
+ addResultSnapshot(result, beforeOrAt);
+ }
+ return;
+ }
+ // -> now both are valid
+
+ // return both => user can interpolate
+ if (isInRange(beforeOrAt->first))
+ {
+ 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";
+ }
+ }
+
static size_t negativeIndexSemantics(long index, size_t size)
{
diff --git a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
index 1feed487fa559e60131a9c80f5caf4a75bb830b1..b80a0a07c82ac6ee452b7cab489cee1f12e11b3b 100644
--- a/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_objects/client/articulated_object/Reader.cpp
@@ -103,7 +103,7 @@ namespace armarx::armem::articulated_object
if (not description)
{
- ARMARX_ERROR << "Unknown object " << name;
+ ARMARX_WARNING << "Unknown object " << name;
return std::nullopt;
}
diff --git a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
index de5b249e4a252a5f39bf44953a5f56dd3e1e32ac..54f45eca1eff23836f9a1db751c1261df157a5d7 100644
--- a/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_objects/server/articulated_object_instance/Visu.cpp
@@ -128,7 +128,7 @@ namespace armarx::armem::server::obj::articulated_object_instance
{
{
// std::scoped_lock lock(visuMutex);
- // ARMARX_IMPORTANT << "Update task";
+ ARMARX_DEBUG << "Update task";
if (p.enabled)
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp
index dc30cbeb47cfa5866eb86cd17134c2b1961a3faf..27fecee2e4f4ae9137a4e7f6281bef679d308da4 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 17c8731dd188d93507553bf9ad521adb60f2cc32..9d0ee4a4975d3d0de6c8c958db659da8aec87c66 100644
--- a/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/client/localization/TransformReader.cpp
@@ -22,8 +22,8 @@
#include "TransformReader.h"
#include "RobotAPI/libraries/armem_robot_state/common/localization/types.h"
+#include "RobotAPI/libraries/armem/core/Time.h"
-#include <Eigen/src/Geometry/Transform.h>
#include <algorithm>
#include <iterator>
#include <numeric>
@@ -99,7 +99,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,
@@ -134,7 +134,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);
@@ -148,7 +148,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 a4c2cd4523670a6f335294da0184358f2237591b..3d7bf59eef0f97f18cf58087522630f87c9b0cbd 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 5703a15c80421e1e05f3a97143bd90219d6cc447..2327c141840bc74f8b43ab14df03b03f2b98ccdf 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 e8afaf3e4d853da674ade877200a4de1b672bf84..ae35822c6080701195c0e6d7d8b6c8c4ad282684 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 bc4f7e897e05ec7d945158de62c3bcba4f11d603..c8db83835ccc53f21e416b723745892a1455f801 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"
@@ -19,38 +20,38 @@
namespace armarx::armem::common::robot_state::localization
{
- TransformResult TransformHelper::lookupTransform(const armem::wm::CoreSegment &localizationCoreSegment,
- const TransformQuery &query)
+ TransformResult TransformHelper::lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
const std::vector<std::string> tfChain =
- buildTransformChain(localizationCoreSegment, query);
+ buildTransformChain(localizationCoreSegment, query);
if (tfChain.empty())
- {
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Cannot create tf lookup chain '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "'"};
- }
+ {
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" +
+ query.header.parentFrame + " -> " + query.header.frame +
+ "'"};
+ }
const std::vector<Eigen::Affine3f> transforms = obtainTransforms(
- localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+ localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
if (transforms.empty())
- {
- ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
- " -> " + query.header.frame +
- "'. No memory data in time range."};
- }
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "No transform available.";
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
+ " -> " + query.header.frame +
+ "'. No memory data in time range."};
+ }
const Eigen::Affine3f transform = std::accumulate(transforms.begin(),
- transforms.end(),
- Eigen::Affine3f::Identity(),
- std::multiplies<>());
+ transforms.end(),
+ Eigen::Affine3f::Identity(),
+ std::multiplies<>());
ARMARX_DEBUG << "Found valid transform";
@@ -59,79 +60,82 @@ namespace armarx::armem::common::robot_state::localization
}
std::vector<std::string>
- TransformHelper::buildTransformChain(const armem::wm::CoreSegment &localizationCoreSegment,
- const TransformQuery &query)
+ TransformHelper::buildTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
ARMARX_DEBUG << "Building transform chain";
- auto join = [](const std::string &parentFrame, const std::string &frame) {
+ auto join = [](const std::string & parentFrame, const std::string & frame)
+ {
return parentFrame + "," + frame;
};
std::vector<std::string> chain;
- const auto &agentProviderSegment =
- localizationCoreSegment.getProviderSegment(query.header.agent);
+ const auto& agentProviderSegment =
+ localizationCoreSegment.getProviderSegment(query.header.agent);
- auto addToChain = [&](const std::string &parentFrame, const std::string &frame) {
+ auto addToChain = [&](const std::string & parentFrame, const std::string & frame)
+ {
const auto entityName = join(parentFrame, frame);
if (agentProviderSegment.hasEntity(entityName))
- {
- chain.push_back(entityName);
- }
+ {
+ chain.push_back(entityName);
+ }
else
- {
- ARMARX_WARNING << deactivateSpam(1) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
- << "'";
- }
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Cannot perform tf lookup '" << parentFrame << " -> " << frame
+ << "'";
+ }
};
- std::array<std::string, 3> knownChain{
- GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
+ std::array<std::string, 3> knownChain
+ {
+ GlobalFrame, MapFrame, OdometryFrame}; // Robot comes next
- auto *frameBeginIt =
- std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
- auto *const frameEndIt =
- std::find(knownChain.begin(), knownChain.end(), query.header.frame);
+ auto* frameBeginIt =
+ std::find(knownChain.begin(), knownChain.end(), query.header.parentFrame);
+ auto* const frameEndIt =
+ std::find(knownChain.begin(), knownChain.end(), query.header.frame);
if (frameBeginIt == knownChain.end())
- {
- ARMARX_WARNING << "Parent frame '" << query.header.parentFrame << "' unknown";
- return {};
- }
+ {
+ ARMARX_WARNING << "Parent frame '" << query.header.parentFrame << "' unknown";
+ return {};
+ }
if (frameEndIt == knownChain.end())
- {
- ARMARX_DEBUG << "Frame '" << query.header.frame << "' must be robot frame";
- }
+ {
+ ARMARX_DEBUG << "Frame '" << query.header.frame << "' must be robot frame";
+ }
const size_t nFrames = std::distance(frameBeginIt, frameEndIt);
ARMARX_DEBUG << "Lookup '" << query.header.parentFrame << " -> " << query.header.frame
<< "' across " << nFrames << " frames";
for (; frameBeginIt != knownChain.end() - 1; frameBeginIt++)
- {
- addToChain(*frameBeginIt, *(frameBeginIt + 1));
- }
+ {
+ addToChain(*frameBeginIt, *(frameBeginIt + 1));
+ }
if (frameEndIt == knownChain.end())
- {
- addToChain(knownChain.back(), query.header.frame);
- }
+ {
+ addToChain(knownChain.back(), query.header.frame);
+ }
if (chain.empty())
- {
- ARMARX_WARNING << deactivateSpam(1) << "Cannot create tf lookup chain '" << query.header.parentFrame
- << " -> " << query.header.frame << "'";
- return {};
- }
+ {
+ ARMARX_WARNING << deactivateSpam(1) << "Cannot create tf lookup chain '" << query.header.parentFrame
+ << " -> " << query.header.frame << "'";
+ return {};
+ }
return chain;
}
inline ::armarx::armem::robot_state::Transform
- convertEntityToTransform(const armem::wm::EntityInstance &item)
+ convertEntityToTransform(const armem::wm::EntityInstance& item)
{
arondto::Transform aronTransform;
aronTransform.fromAron(item.data());
@@ -143,21 +147,34 @@ namespace armarx::armem::common::robot_state::localization
}
auto
- findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform> &transforms,
- const int64_t timestamp)
+ findFirstElementAfter(const std::vector<::armarx::armem::robot_state::Transform>& transforms,
+ const armem::Time& timestamp)
{
+ const auto comp = [](const armem::Time & timestamp, const auto & transform)
+ {
+ return transform.header.timestamp < timestamp;
+ };
+
+ const auto it = std::upper_bound(transforms.begin(), transforms.end(), timestamp, comp);
+
auto timestampBeyond =
- [timestamp](const ::armarx::armem::robot_state::Transform &transform) {
- return transform.header.timestamp > timestamp;
- };
+ [timestamp](const ::armarx::armem::robot_state::Transform & transform)
+ {
+ return transform.header.timestamp > timestamp;
+ };
const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
+
+ ARMARX_WARNING << "Fooasdfasdjfh";
+
+ ARMARX_CHECK(it == poseNextIt);
+
return poseNextIt;
}
Eigen::Affine3f
- interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform> &queue,
- int64_t timestamp)
+ interpolateTransform(const std::vector<::armarx::armem::robot_state::Transform>& queue,
+ armem::Time timestamp)
{
ARMARX_TRACE;
@@ -192,21 +209,21 @@ 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 ×tamp)
+ TransformHelper::obtainTransforms(const armem::wm::CoreSegment& localizationCoreSegment,
+ const std::vector<std::string>& tfChain,
+ const std::string& agent,
+ const armem::Time& timestamp)
{
- const auto &agentProviderSegment = localizationCoreSegment.getProviderSegment(agent);
+ const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(agent);
ARMARX_DEBUG << "Provider segments"
<< simox::alg::get_keys(localizationCoreSegment.providerSegments());
@@ -214,43 +231,48 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_DEBUG << "Entities: " << simox::alg::get_keys(agentProviderSegment.entities());
try
+ {
+ std::vector<Eigen::Affine3f> transforms;
+ transforms.reserve(tfChain.size());
+ std::transform(tfChain.begin(),
+ tfChain.end(),
+ std::back_inserter(transforms),
+ [&](const std::string & entityName)
{
- std::vector<Eigen::Affine3f> transforms;
- transforms.reserve(tfChain.size());
- std::transform(tfChain.begin(),
- tfChain.end(),
- std::back_inserter(transforms),
- [&](const std::string &entityName) {
- return obtainTransform(
- entityName, agentProviderSegment, timestamp);
- });
- return transforms;
- }
- catch (const armem::error::MissingEntry &missingEntryError)
- {
- ARMARX_WARNING << missingEntryError.what();
- }
- catch (const ::armarx::exceptions::local::ExpressionException &ex)
- {
- ARMARX_WARNING << "local exception: " << ex.what();
- }
+ return obtainTransform(
+ entityName, agentProviderSegment, timestamp);
+ });
+ return transforms;
+ }
+ catch (const armem::error::MissingEntry& missingEntryError)
+ {
+ ARMARX_WARNING << missingEntryError.what();
+ }
+ catch (const ::armarx::exceptions::local::ExpressionException& ex)
+ {
+ ARMARX_WARNING << "local exception: " << ex.what();
+ }
+ catch (...)
+ {
+ ARMARX_WARNING << "Error: " << GetHandledExceptionString();
+ }
+
- ARMARX_WARNING << "Unkown error.";
return {};
}
Eigen::Affine3f
- TransformHelper::obtainTransform(const std::string &entityName,
- const armem::wm::ProviderSegment &agentProviderSegment,
- const int64_t timestamp)
+ TransformHelper::obtainTransform(const std::string& entityName,
+ const armem::wm::ProviderSegment& agentProviderSegment,
+ const armem::Time& timestamp)
{
ARMARX_DEBUG << "getEntity:" + entityName;
- const auto &entity = agentProviderSegment.getEntity(entityName);
+ const auto& entity = agentProviderSegment.getEntity(entityName);
ARMARX_DEBUG << "History (size: " << entity.history().size() << ")"
- << simox::alg::get_keys(entity.history());
+ << simox::alg::get_keys(entity.history());
// if (entity.history.empty())
// {
@@ -264,31 +286,34 @@ namespace armarx::armem::common::robot_state::localization
const auto entitySnapshots = simox::alg::get_values(entity.history());
std::transform(
- entitySnapshots.begin(),
- entitySnapshots.end(),
- std::back_inserter(transforms),
- [](const auto &entity) { return convertEntityToTransform(entity.getInstance(0)); });
+ entitySnapshots.begin(),
+ entitySnapshots.end(),
+ std::back_inserter(transforms),
+ [](const auto & entity)
+ {
+ return convertEntityToTransform(entity.getInstance(0));
+ });
ARMARX_DEBUG << "obtaining transform";
if (transforms.size() > 1)
- {
- // TODO(fabian.reister): remove
- return transforms.front().transform;
+ {
+ // TODO(fabian.reister): remove
+ return transforms.front().transform;
- ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
- const auto p = interpolateTransform(transforms, timestamp);
- ARMARX_DEBUG << "Done interpolating transform";
- return p;
- }
+ ARMARX_DEBUG << "More than one snapshots received: " << transforms.size();
+ const auto p = interpolateTransform(transforms, timestamp);
+ ARMARX_DEBUG << "Done interpolating transform";
+ return p;
+ }
// accept this to fail (will raise armem::error::MissingEntry)
if (transforms.empty())
- {
- ARMARX_DEBUG << "empty transform";
+ {
+ ARMARX_DEBUG << "empty transform";
- throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
- }
+ throw armem::error::MissingEntry("foo", "bar", "foo2", "bar2");
+ }
ARMARX_DEBUG << "single transform";
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 7505dd3859be919000e5bb80b1c9b6aaddb7ddd4..8045c552e839b657c47dca5aabe6f76e0c384d0c 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/description/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
index 6cfb5583ef2882ac6416abd639fb95fa28b47545..826c5b484768686e2edb8cee409df4136bcc85ab 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/description/Segment.cpp
@@ -33,7 +33,7 @@ namespace armarx::armem::server::robot_state::description
std::mutex& memoryMutex) :
iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
{
- Logging::setTag("ArticulatedObjectInstanceSegment");
+ Logging::setTag("DescriptionSegment");
}
Segment::~Segment() = default;
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 3bf5ef05f0c735a02050b8c5c77f91b07f299f68..6858f0c346dabebf96b9c2337d85760651abbbc8 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/localization/Segment.cpp
@@ -39,7 +39,7 @@ namespace armarx::armem::server::robot_state::localization
std::mutex& memoryMutex) :
iceMemory(memoryToIceAdapter), memoryMutex(memoryMutex)
{
- Logging::setTag("ArticulatedObjectInstanceSegment");
+ Logging::setTag("LocalizationSegment");
}
Segment::~Segment() = default;
@@ -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/server/localization/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.cpp
deleted file mode 100644
index e882416abe4c6c5dcfc65aba55a693e69b4e244c..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.cpp
+++ /dev/null
@@ -1,170 +0,0 @@
-#include "Visu.h"
-#include "ArmarXCore/core/logging/Logging.h"
-#include "ArmarXCore/core/time/CycleUtil.h"
-
-#include <algorithm>
-
-#include <SimoxUtility/math/pose.h>
-
-#include <ArmarXCore/core/time/TimeUtil.h>
-
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-
-#include "Segment.h"
-
-namespace armarx::armem::server::obj::articulated_object_instance
-{
-
- void Visu::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
- {
- defs->optional(p.enabled, prefix + "enabled",
- "Enable or disable visualization of objects.");
- defs->optional(p.frequencyHz, prefix + "frequenzyHz",
- "Frequency of visualization.");
- }
-
-
- viz::Layer Visu::visualizeProvider(
- const std::string& providerName,
- const armarx::armem::articulated_object::ArticulatedObjects& objects) const
- {
- viz::Layer layer = arviz.layer(providerName);
-
- visualizeRobots(layer, objects);
-
- return layer;
- }
-
-
- void Visu::visualizeR(viz::Layer& layer, const armarx::armem::articulated_object::ArticulatedObjects& objects) const
- {
- const auto visualizeObject = [&](const armarx::armem::articulated_object::ArticulatedObject & obj)
- {
-
- const auto xmlPath = obj.description.xml.serialize();
-
- // clang-format off
- auto robot = viz::Robot(obj.description.name)
- // .file(xmlPath.package, xmlPath.path)
- .file("ArmarXObjects", "./data/ArmarXObjects/Environment/mobile-kitchen/dishwasher-only/dishwasher.xml")
- .joints(obj.config.jointMap)
- .pose(obj.config.globalPose);
-
- robot.useFullModel();
- // clang-format on
-
- layer.add(robot);
- };
-
- std::for_each(objects.begin(), objects.end(), visualizeObject);
-
- }
-
- void Visu::init()
- {
- updateTask = new SimpleRunningTask<>([this]()
- {
- this->visualizeRun();
- });
- updateTask->start();
- }
-
-
- // void Visu::RemoteGui::setup(const Visu& visu)
- // {
- // using namespace armarx::RemoteGui::Client;
-
- // enabled.setValue(visu.enabled);
- // inGlobalFrame.setValue(visu.inGlobalFrame);
- // alpha.setRange(0, 1.0);
- // alpha.setValue(visu.alpha);
- // alphaByConfidence.setValue(visu.alphaByConfidence);
- // oobbs.setValue(visu.oobbs);
- // objectFrames.setValue(visu.objectFrames);
- // {
- // float max = 10000;
- // objectFramesScale.setRange(0, max);
- // objectFramesScale.setDecimals(2);
- // objectFramesScale.setSteps(int(10 * max));
- // objectFramesScale.setValue(visu.objectFramesScale);
- // }
-
- // GridLayout grid;
- // int row = 0;
- // grid.add(Label("Enabled"), {row, 0}).add(enabled, {row, 1});
- // row++;
- // grid.add(Label("Global Frame"), {row, 0}).add(inGlobalFrame, {row, 1});
- // row++;
- // grid.add(Label("Alpha"), {row, 0}).add(alpha, {row, 1}, {1, 3});
- // row++;
- // grid.add(Label("Alpha by Confidence"), {row, 0}).add(alphaByConfidence, {row, 1});
- // row++;
- // grid.add(Label("OOBB"), {row, 0}).add(oobbs, {row, 1});
- // row++;
- // grid.add(Label("Object Frames"), {row, 0}).add(objectFrames, {row, 1});
- // grid.add(Label("Scale:"), {row, 2}).add(objectFramesScale, {row, 3});
- // row++;
-
- // group.setLabel("Visualization");
- // group.addChild(grid);
- // }
-
- // void Visu::RemoteGui::update(Visu& visu)
- // {
- // visu.enabled = enabled.getValue();
- // visu.inGlobalFrame = inGlobalFrame.getValue();
- // visu.alpha = alpha.getValue();
- // visu.alphaByConfidence = alphaByConfidence.getValue();
- // visu.oobbs = oobbs.getValue();
- // visu.objectFrames = objectFrames.getValue();
- // visu.objectFramesScale = objectFramesScale.getValue();
- // }
-
-
- void Visu::visualizeRun()
- {
- CycleUtil cycle(static_cast<int>(1000 / p.frequencyHz));
- while (updateTask && not updateTask->isStopped())
- {
- {
- // std::scoped_lock lock(visuMutex);
- ARMARX_DEBUG << "Update task";
-
- if (p.enabled)
- {
- // TIMING_START(Visu);
-
- const auto articulatedObjects = segment.getArticulatedObjects();
-
- ARMARX_INFO << "Found " << articulatedObjects.size() << " articulated objects";
-
- viz::Layer layer = arviz.layer("ArticulatedObjectInstances");
-
- ARMARX_INFO << "visualizing objects";
- visualizeObjects(layer, articulatedObjects);
-
- ARMARX_INFO << "Committing objects";
-
- arviz.commit({layer});
-
- ARMARX_INFO << "Done committing";
-
-
- // TIMING_END_STREAM(Visu, ARMARX_VERBOSE);
-
- // if (debugObserver)
- // {
- // debugObserver->setDebugChannel(getName(),
- // {
- // { "t Visualize [ms]", new Variant(Visu.toMilliSecondsDouble()) },
- // });
- // }
- }
- }
- cycle.waitForCycleDuration();
- }
- }
-
-
-
-} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.h
deleted file mode 100644
index 54aea5e09f0d3000596c83d2eb8ffbafcbf8b81e..0000000000000000000000000000000000000000
--- a/source/RobotAPI/libraries/armem_robot_state/server/localization/Visu.h
+++ /dev/null
@@ -1,101 +0,0 @@
-/*
- * 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 <ArmarXCore/core/logging/Logging.h>
-#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-
-// #include <ArmarXGui/libraries/RemoteGui/Client/Widgets.h>
-
-#include <RobotAPI/components/ArViz/Client/Client.h>
-
-#include <RobotAPI/libraries/armem_objects/types.h>
-
-
-namespace armarx
-{
- class ObjectFinder;
-}
-namespace armarx::armem::server::obj::articulated_object_instance
-{
- class Segment;
-
- /**
- * @brief Models decay of object localizations by decreasing the confidence
- * the longer the object was not localized.
- */
- class Visu : public armarx::Logging
- {
- public:
-
- Visu(const viz::Client& arviz, const Segment& segment): arviz(arviz), segment(segment) {}
-
- void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "visu.");
-
- void init();
-
- protected:
- viz::Layer visualizeProvider(
- const std::string& providerName,
- const armarx::armem::articulated_object::ArticulatedObjects& objects
- ) const;
-
- void visualizeObjects(
- viz::Layer& layer,
- const armarx::armem::articulated_object::ArticulatedObjects& objects
- ) const;
-
-
- private:
- viz::Client arviz;
- const Segment& segment;
-
- struct Properties
- {
- bool enabled = true;
- float frequencyHz = 25;
- } p;
-
-
- SimpleRunningTask<>::pointer_type updateTask;
- void visualizeRun();
-
- // struct RemoteGui
- // {
- // armarx::RemoteGui::Client::GroupBox group;
-
- // armarx::RemoteGui::Client::CheckBox enabled;
-
- // armarx::RemoteGui::Client::CheckBox inGlobalFrame;
- // armarx::RemoteGui::Client::FloatSlider alpha;
- // armarx::RemoteGui::Client::CheckBox alphaByConfidence;
- // armarx::RemoteGui::Client::CheckBox oobbs;
- // armarx::RemoteGui::Client::CheckBox objectFrames;
- // armarx::RemoteGui::Client::FloatSpinBox objectFramesScale;
-
- // // void setup(const Visu& visu);
- // // void update(Visu& visu);
- // };
-
- };
-
-} // namespace armarx::armem::server::obj::articulated_object_instance
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
index 500925f2d99383ba7e463ff09ecf94ea7283b087..5ec63c12d9c3f6485839ff138b1ebe409dd1adc8 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.cpp
@@ -33,7 +33,7 @@ namespace armarx::armem::server::robot_state::proprioception
iceMemory(memoryToIceAdapter),
memoryMutex(memoryMutex)
{
- Logging::setTag("ArticulatedObjectInstanceSegment");
+ Logging::setTag("ProprioceptionSegment");
}
Segment::~Segment() = default;
diff --git a/source/RobotAPI/libraries/armem_robot_state/types.h b/source/RobotAPI/libraries/armem_robot_state/types.h
index 43408df40c236f0fc4e877f1a04cd4a23ee818e3..39daba5d2d7db0148cb9df264d8da2acac54d036 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 ea25e01e3517602df7cf7871cd393d5639de8d50..2cd6b7eb906d4b85cf6527979eb9573f4d9568ea 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 985409745af2576b980de11b169809efd91118b2..48c24690503cec91a17da37909a53c3228984e26 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