diff --git a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
index 14aceaef8913538df4af2e5272abd125150ca352..3ab4e0530b54331305cdbc8f8ad05603607b0ac9 100644
--- a/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+++ b/source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
@@ -20,13 +20,13 @@
* GNU General Public License
*/
+#include "RobotUnitDataStreamingReceiver.h"
+
#include <Ice/ObjectAdapter.h>
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/ArmarXManager.h>
-#include "RobotUnitDataStreamingReceiver.h"
-
namespace armarx::detail::RobotUnitDataStreamingReceiver
{
class Receiver :
@@ -34,23 +34,37 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
virtual public RobotUnitDataStreaming::Receiver
{
public:
- std::string getDefaultName() const override
+ std::string
+ getDefaultName() const override
{
return "RobotUnitDataStreamingReceiver";
}
+
~Receiver()
{
std::lock_guard g{_data_mutex};
}
- void onInitComponent() override {}
- void onConnectComponent() override {}
- void onExitComponent() override {}
-
- void update_async(
- const RobotUnitDataStreaming::AMD_Receiver_updatePtr& ptr,
- const RobotUnitDataStreaming::TimeStepSeq& data,
- Ice::Long msgSequenceNbr,
- const Ice::Current&) override
+
+ void
+ onInitComponent() override
+ {
+ }
+
+ void
+ onConnectComponent() override
+ {
+ }
+
+ void
+ onExitComponent() override
+ {
+ }
+
+ void
+ update_async(const RobotUnitDataStreaming::AMD_Receiver_updatePtr& ptr,
+ const RobotUnitDataStreaming::TimeStepSeq& data,
+ Ice::Long msgSequenceNbr,
+ const Ice::Current&) override
{
ptr->ice_response();
if (_discard_data)
@@ -65,32 +79,31 @@ namespace armarx::detail::RobotUnitDataStreamingReceiver
_data[seq] = data;
}
- std::atomic_bool _discard_data = false;
- std::mutex _data_mutex;
- std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> _data;
- Ice::Identity _identity;
+ std::atomic_bool _discard_data = false;
+ std::mutex _data_mutex;
+ std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> _data;
+ Ice::Identity _identity;
};
-}
+} // namespace armarx::detail::RobotUnitDataStreamingReceiver
namespace armarx
{
RobotUnitDataStreamingReceiver::RobotUnitDataStreamingReceiver(
const ManagedIceObjectPtr& obj,
const RobotUnitInterfacePrx& ru,
- const RobotUnitDataStreaming::Config& cfg
- ) : _obj{obj}, _ru{ru}
+ const RobotUnitDataStreaming::Config& cfg) :
+ _obj{obj}, _ru{ru}
{
ARMARX_CHECK_NOT_NULL(_obj);
ARMARX_CHECK_NOT_NULL(_ru);
_receiver = make_shared<detail::RobotUnitDataStreamingReceiver::Receiver>();
- _receiver->_identity.name =
- _obj->getName() + "_RobotUnitDataStreamingReceiver_" +
- std::to_string(clock_t::now().time_since_epoch().count());
+ _receiver->_identity.name = _obj->getName() + "_RobotUnitDataStreamingReceiver_" +
+ std::to_string(clock_t::now().time_since_epoch().count());
auto adapter = _obj->getArmarXManager()->getAdapter();
_proxy = RobotUnitDataStreaming::ReceiverPrx::uncheckedCast(
- adapter->add(_receiver, _receiver->_identity));
+ adapter->add(_receiver, _receiver->_identity));
_description = _ru->startDataStreaming(_proxy, cfg);
}
@@ -120,14 +133,15 @@ namespace armarx
ARMARX_INFO << deactivateSpam() << "waiting until receiver is removed from ice";
}
}
- _proxy = nullptr;
+ _proxy = nullptr;
_receiver = nullptr;
}
- std::deque<RobotUnitDataStreaming::TimeStep>& RobotUnitDataStreamingReceiver::getDataBuffer()
+ std::deque<RobotUnitDataStreaming::TimeStep>&
+ RobotUnitDataStreamingReceiver::getDataBuffer()
{
ARMARX_CHECK_NOT_NULL(_receiver);
- std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> data;
+ std::map<std::uint64_t, RobotUnitDataStreaming::TimeStepSeq> data;
{
std::lock_guard g{_receiver->_data_mutex};
std::swap(data, _receiver->_data);
@@ -169,14 +183,12 @@ namespace armarx
_tmp_data_buffer_seq_id = it->first;
for (auto& step : it->second)
{
- if (
- _last_iteration_id != -1 &&
- _last_iteration_id + 1 != step.iterationId
- )
+ if (_last_iteration_id != -1 && _last_iteration_id + 1 != step.iterationId)
{
- ARMARX_VERBOSE << "Missing Iterations or iterations out of order! "
- << "This should not happen. "
- << VAROUT(_last_iteration_id) << ", " << VAROUT(step.iterationId);
+ ARMARX_INFO << deactivateSpam(10)
+ << "Missing Iterations or iterations out of order! "
+ << "This should not happen. " << VAROUT(_last_iteration_id) << ", "
+ << VAROUT(step.iterationId);
}
_last_iteration_id = step.iterationId;
_data_buffer.emplace_back(std::move(step));
@@ -186,12 +198,14 @@ namespace armarx
return _data_buffer;
}
- const RobotUnitDataStreaming::DataStreamingDescription& RobotUnitDataStreamingReceiver::getDataDescription() const
+ const RobotUnitDataStreaming::DataStreamingDescription&
+ RobotUnitDataStreamingReceiver::getDataDescription() const
{
return _description;
}
- std::string RobotUnitDataStreamingReceiver::getDataDescriptionString() const
+ std::string
+ RobotUnitDataStreamingReceiver::getDataDescriptionString() const
{
std::stringstream str;
const auto& entr = getDataDescription().entries;
@@ -202,4 +216,4 @@ namespace armarx
}
return str.str();
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
index 22ae30fbd8489aff451fda127a170f58548f5fcc..45b26e7c9e8c4dd124b762d297d2fb0dca29a317 100644
--- a/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
+++ b/source/RobotAPI/libraries/armem/client/util/SimpleReaderBase.h
@@ -54,6 +54,10 @@ namespace armarx::armem::client::util
virtual void connect();
const Properties& properties() const;
+ void setProperties(const Properties& p)
+ {
+ props = p;
+ }
protected:
diff --git a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
index bf139a68a7a071bb38dac9e018eca9251b29cc92..ca641d541ac01430350b5e1e72998545c6c82805 100644
--- a/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
+++ b/source/RobotAPI/libraries/armem_locations/client/Reader.cpp
@@ -52,18 +52,22 @@ namespace armarx::armem::locations::client
std::map<std::string, armarx::navigation::location::arondto::Location>
Reader::getAllLocations()
{
- auto mId = armarx::armem::MemoryID{p.memoryName, "Locations"};
+ auto mId = armarx::armem::MemoryID{p.memoryName, "Location"};
auto res = locationReader.getLatestSnapshotsIn(mId);
if (res.success)
{
std::map<std::string, armarx::navigation::location::arondto::Location> ret;
- res.memory.forEachInstance(
- [&ret](const armarx::armem::wm::EntityInstance& i)
+ res.memory.forEachEntity(
+ [&ret](const armarx::armem::wm::Entity& e)
{
- auto loc = i.dataAs<armarx::navigation::location::arondto::Location>();
- ret[i.id().entityName] = loc;
- });
+ auto i = e.findLatestInstance();
+ if (i)
+ {
+ auto loc = i->dataAs<armarx::navigation::location::arondto::Location>();
+ ret[i->id().entityName] = loc;
+ }
+ });
return ret;
}
diff --git a/source/RobotAPI/libraries/armem_locations/frame_conversions.h b/source/RobotAPI/libraries/armem_locations/frame_conversions.h
index b715c062448cf1d334e4e8503e39658cf60b0454..876e6f9529fb83c88c5f7575daed6c7437f4b3f3 100644
--- a/source/RobotAPI/libraries/armem_locations/frame_conversions.h
+++ b/source/RobotAPI/libraries/armem_locations/frame_conversions.h
@@ -37,7 +37,7 @@ namespace armarx::armem::locations
return;
}
- if (simox::alg::starts_with(framedLoc.framedPose.header.agent, objPose.objectID.str()))
+ if (simox::alg::starts_with(objPose.objectID.str(), framedLoc.framedPose.header.agent))
{
ARMARX_CHECK(framedLoc.framedPose.header.frame ==
"root"); // TODO: Right now we only allow root
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 e808a4b2e31f7d3763b0a0a8052502eb77260f27..7e3908c5503d4ead6a018f9e9555618b01ca2d28 100644
--- a/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/common/localization/TransformHelper.cpp
@@ -1,4 +1,5 @@
#include "TransformHelper.h"
+
#include <optional>
#include <string>
@@ -6,55 +7,53 @@
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <SimoxUtility/math/pose/interpolate.h>
-#include "ArmarXCore/core/logging/Logging.h"
-#include <ArmarXCore/core/exceptions/LocalException.h>
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
#include "ArmarXCore/core/time/DateTime.h"
#include "ArmarXCore/core/time/Duration.h"
-#include "RobotAPI/libraries/armem/core/forward_declarations.h"
+#include <ArmarXCore/core/exceptions/LocalException.h>
+#include "RobotAPI/libraries/armem/core/forward_declarations.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
-#include <RobotAPI/libraries/core/FramedPose.h>
-
-#include <RobotAPI/libraries/aron/common/aron_conversions.h>
-
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/error/ArMemError.h>
#include <RobotAPI/libraries/armem/core/wm/memory_definitions.h>
#include <RobotAPI/libraries/armem/server/wm/memory_definitions.h>
-
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron_conversions.h>
-
+#include <RobotAPI/libraries/aron/common/aron_conversions.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::common::robot_state::localization
{
- template <class ...Args>
+ template <class... Args>
TransformResult
TransformHelper::_lookupTransform(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
const TransformQuery& query)
{
- const std::vector<std::string> tfChain = _buildTransformChain(localizationCoreSegment, query);
+ const std::vector<std::string> tfChain =
+ _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 +
- "' for robot `" + query.header.agent + "`."};
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" + query.header.parentFrame +
+ " -> " + query.header.frame + "' for robot `" +
+ query.header.agent + "`."};
}
const std::vector<Eigen::Affine3f> transforms = _obtainTransforms(
- localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
+ localizationCoreSegment, tfChain, query.header.agent, query.header.timestamp);
- const std::optional<armem::Time> sanitizedTimestamp = _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
+ const std::optional<armem::Time> sanitizedTimestamp =
+ _obtainTimestamp(localizationCoreSegment, query.header.timestamp);
- if(not sanitizedTimestamp.has_value())
+ if (not sanitizedTimestamp.has_value())
{
- return {.transform = {.header = query.header},
- .status = TransformResult::Status::Error,
+ return {.transform = {.header = query.header},
+ .status = TransformResult::Status::Error,
.errorMessage = "Error: Issue with timestamp"};
}
@@ -66,79 +65,65 @@ namespace armarx::armem::common::robot_state::localization
// ARMARX_INFO << header.timestamp << "vs" << sanitizedTimestamp;
header.timestamp = sanitizedTimestamp.value();
-
+
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."};
+ 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<>());
+ const Eigen::Affine3f transform = std::accumulate(
+ transforms.begin(), transforms.end(), Eigen::Affine3f::Identity(), std::multiplies<>());
ARMARX_DEBUG << "Found valid transform";
return {.transform = {.header = header, .transform = transform},
- .status = TransformResult::Status::Success};
+ .status = TransformResult::Status::Success};
}
-
- template <class ...Args>
+ template <class... Args>
TransformChainResult
TransformHelper::_lookupTransformChain(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
const TransformQuery& query)
{
- const std::vector<std::string> tfChain = _buildTransformChain(localizationCoreSegment, query);
+ const std::vector<std::string> tfChain =
+ _buildTransformChain(localizationCoreSegment, query);
if (tfChain.empty())
{
- ARMARX_VERBOSE << "TF chain is empty";
- return
- {
- .header = query.header,
- .transforms = std::vector<Eigen::Affine3f>{},
- .status = TransformChainResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Cannot create tf lookup chain '" +
- query.header.parentFrame + " -> " + query.header.frame +
- "' for robot `" + query.header.agent + "`."
- };
+ ARMARX_DEBUG << "TF chain is empty";
+ return {.header = query.header,
+ .transforms = std::vector<Eigen::Affine3f>{},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Cannot create tf lookup chain '" + query.header.parentFrame +
+ " -> " + query.header.frame + "' for robot `" +
+ query.header.agent + "`."};
}
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
- {
- .header = query.header,
- .transforms = {},
- .status = TransformChainResult::Status::ErrorFrameNotAvailable,
- .errorMessage = "Error in TF loookup: '" + query.header.parentFrame +
- " -> " + query.header.frame +
- "'. No memory data in time range."
- };
+ return {.header = query.header,
+ .transforms = {},
+ .status = TransformChainResult::Status::ErrorFrameNotAvailable,
+ .errorMessage = "Error in TF loookup: '" + query.header.parentFrame + " -> " +
+ query.header.frame + "'. No memory data in time range."};
}
ARMARX_DEBUG << "Found valid transform";
- return
- {
- .header = query.header,
- .transforms = transforms,
- .status = TransformChainResult::Status::Success
- };
+ return {.header = query.header,
+ .transforms = transforms,
+ .status = TransformChainResult::Status::Success};
}
-
- template <class ...Args>
+ template <class... Args>
std::vector<std::string>
TransformHelper::_buildTransformChain(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
@@ -148,14 +133,15 @@ namespace armarx::armem::common::robot_state::localization
std::vector<std::string> chain;
- const auto& agentProviderSegment = localizationCoreSegment.getProviderSegment(query.header.agent);
+ const auto& agentProviderSegment =
+ localizationCoreSegment.getProviderSegment(query.header.agent);
const std::vector<std::string> tfs = agentProviderSegment.getEntityNames();
// lookup from robot root to global
std::map<std::string, std::string> tfLookup;
- for(const std::string& tf: tfs)
+ for (const std::string& tf : tfs)
{
const auto frames = simox::alg::split(tf, ",");
ARMARX_CHECK_EQUAL(frames.size(), 2);
@@ -165,60 +151,66 @@ namespace armarx::armem::common::robot_state::localization
std::string currentFrame = query.header.parentFrame;
chain.push_back(currentFrame);
- while(tfLookup.count(currentFrame) > 0 and currentFrame != query.header.frame)
+ while (tfLookup.count(currentFrame) > 0 and currentFrame != query.header.frame)
{
currentFrame = tfLookup.at(currentFrame);
chain.push_back(currentFrame);
}
-
+
ARMARX_DEBUG << VAROUT(chain);
if (chain.empty() or chain.back() != query.header.frame)
{
- ARMARX_WARNING << deactivateSpam(60) << "Cannot create tf lookup chain '" << query.header.parentFrame
- << " -> " << query.header.frame << "' for robot `" + query.header.agent + "`.";
+ ARMARX_WARNING << deactivateSpam(60) << "Cannot create tf lookup chain '"
+ << query.header.parentFrame << " -> " << query.header.frame
+ << "' for robot `" + query.header.agent + "`.";
return {};
}
std::vector<std::string> frameChain;
- for(size_t i = 0; i < (chain.size() - 1); i++)
+ for (size_t i = 0; i < (chain.size() - 1); i++)
{
- frameChain.push_back(chain.at(i) + "," + chain.at(i+1));
+ frameChain.push_back(chain.at(i) + "," + chain.at(i + 1));
}
return frameChain;
}
- template <class ...Args>
+ template <class... Args>
std::optional<armarx::core::time::DateTime>
- TransformHelper::_obtainTimestamp(const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment, const armem::Time& timestamp)
+ TransformHelper::_obtainTimestamp(
+ const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
+ const armem::Time& timestamp)
{
-
+
// first we check which the newest timestamp is
std::optional<int64_t> timeSinceEpochUs = std::nullopt;
- localizationCoreSegment.forEachEntity([&timeSinceEpochUs, ×tamp](const auto& entity){
- auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
-
- if(snapshot == nullptr)
+ localizationCoreSegment.forEachEntity(
+ [&timeSinceEpochUs, ×tamp](const auto& entity)
{
- return;
- }
+ auto snapshot = entity.findLatestSnapshotBeforeOrAt(timestamp);
- if(not snapshot->hasInstance(0))
- {
- return;
- }
+ if (snapshot == nullptr)
+ {
+ return;
+ }
+
+ if (not snapshot->hasInstance(0))
+ {
+ return;
+ }
- const armem::wm::EntityInstance& item = snapshot->getInstance(0);
- const auto tf = _convertEntityToTransform(item);
-
- const auto& dataTs = tf.header.timestamp;
+ const armem::wm::EntityInstance& item = snapshot->getInstance(0);
+ const auto tf = _convertEntityToTransform(item);
- timeSinceEpochUs = std::max(timeSinceEpochUs.value_or(0), dataTs.toMicroSecondsSinceEpoch());
- });
+ const auto& dataTs = tf.header.timestamp;
- if(not timeSinceEpochUs.has_value())
+ timeSinceEpochUs =
+ std::max(timeSinceEpochUs.value_or(0), dataTs.toMicroSecondsSinceEpoch());
+ });
+
+ if (not timeSinceEpochUs.has_value())
{
return std::nullopt;
}
@@ -226,11 +218,11 @@ namespace armarx::armem::common::robot_state::localization
// then we ensure that the timestamp is not more recent than the query timestamp
timeSinceEpochUs = std::min(timeSinceEpochUs.value(), timestamp.toMicroSecondsSinceEpoch());
- return armarx::core::time::DateTime(armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
+ return armarx::core::time::DateTime(
+ armarx::core::time::Duration::MicroSeconds(timeSinceEpochUs.value()));
}
-
- template <class ...Args>
+ template <class... Args>
std::vector<Eigen::Affine3f>
TransformHelper::_obtainTransforms(
const armem::base::CoreSegmentBase<Args...>& localizationCoreSegment,
@@ -250,10 +242,9 @@ namespace armarx::armem::common::robot_state::localization
std::transform(tfChain.begin(),
tfChain.end(),
std::back_inserter(transforms),
- [&](const std::string & entityName)
- {
- return _obtainTransform(entityName, agentProviderSegment, timestamp);
- });
+ [&](const std::string& entityName) {
+ return _obtainTransform(entityName, agentProviderSegment, timestamp);
+ });
return transforms;
}
catch (const armem::error::MissingEntry& missingEntryError)
@@ -276,8 +267,7 @@ namespace armarx::armem::common::robot_state::localization
return {};
}
-
- template <class ...Args>
+ template <class... Args>
Eigen::Affine3f
TransformHelper::_obtainTransform(
const std::string& entityName,
@@ -327,7 +317,6 @@ namespace armarx::armem::common::robot_state::localization
return transforms.front().transform;
}
-
::armarx::armem::robot_state::Transform
TransformHelper::_convertEntityToTransform(const armem::wm::EntityInstance& item)
{
@@ -344,18 +333,13 @@ namespace armarx::armem::common::robot_state::localization
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 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;
- };
+ auto timestampBeyond = [timestamp](const ::armarx::armem::robot_state::Transform& transform)
+ { return transform.header.timestamp > timestamp; };
const auto poseNextIt = std::find_if(transforms.begin(), transforms.end(), timestampBeyond);
@@ -374,7 +358,7 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_DEBUG << "Entering";
ARMARX_CHECK(not queue.empty())
- << "The queue has to contain at least two items to perform a lookup";
+ << "The queue has to contain at least two items to perform a lookup";
ARMARX_DEBUG << "Entering ... "
<< "Q front " << queue.front().header.timestamp << " "
@@ -384,11 +368,11 @@ namespace armarx::armem::common::robot_state::localization
// TODO(fabian.reister): sort queue.
ARMARX_CHECK(queue.back().header.timestamp > timestamp)
- << "Cannot perform lookup into the future!";
+ << "Cannot perform lookup into the future!";
// ARMARX_DEBUG << "Entering 1.5 " << queue.front().timestamp << " " << timestamp;
ARMARX_CHECK(queue.front().header.timestamp < timestamp)
- << "Cannot perform lookup. Timestamp too old";
+ << "Cannot perform lookup. Timestamp too old";
// => now we know that there is an element right after and before the timestamp within our queue
ARMARX_DEBUG << "Entering 2";
@@ -402,36 +386,39 @@ namespace armarx::armem::common::robot_state::localization
ARMARX_DEBUG << "deref";
// the time fraction [0..1] of the lookup wrt to posePre and poseNext
- const double t = (timestamp - posePreIt->header.timestamp).toMicroSecondsDouble() /
- (poseNextIt->header.timestamp - posePreIt->header.timestamp).toMicroSecondsDouble();
+ 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, static_cast<float>(t));
+ return simox::math::interpolatePose(
+ posePreIt->transform, poseNextIt->transform, static_cast<float>(t));
}
-
- TransformResult TransformHelper::lookupTransform(
- const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+ TransformResult
+ TransformHelper::lookupTransform(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
return _lookupTransform(localizationCoreSegment, query);
}
- TransformResult TransformHelper::lookupTransform(
- const armem::server::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+
+ TransformResult
+ TransformHelper::lookupTransform(const armem::server::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
return _lookupTransform(localizationCoreSegment, query);
}
-
- TransformChainResult TransformHelper::lookupTransformChain(
- const armem::wm::CoreSegment& localizationCoreSegment,
- const TransformQuery& query)
+ TransformChainResult
+ TransformHelper::lookupTransformChain(const armem::wm::CoreSegment& localizationCoreSegment,
+ const TransformQuery& query)
{
return _lookupTransformChain(localizationCoreSegment, query);
}
- TransformChainResult TransformHelper::lookupTransformChain(
+
+ TransformChainResult
+ TransformHelper::lookupTransformChain(
const armem::server::wm::CoreSegment& localizationCoreSegment,
const TransformQuery& query)
{
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
index b8b3190d1bae586226c60b490bb399abe5799272..41eaf6e2ec2f2bcead697ec6ca7e86868bca44c4 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.cpp
@@ -117,6 +117,24 @@ namespace armarx::armem::server::robot_state
}
}
+ void Visu::visualizeFramesIndividual(
+ viz::Layer& layerFrames,
+ const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames)
+ {
+
+ std::vector<std::string> FRAME_NAMES{/*world*/ "map", "odom", "robot"};
+
+ for (const auto& [robotName, robotFrames] : frames)
+ {
+ int i = 0;
+ for (const auto& frame : robotFrames)
+ {
+ layerFrames.add(viz::Pose(robotName + FRAME_NAMES.at(i++)).pose(frame.matrix()).scale(3));
+ }
+ }
+ }
+
+
void Visu::visualizeRun()
{
@@ -213,6 +231,12 @@ namespace armarx::armem::server::robot_state
TIMING_END_STREAM(tVisuBuildLayers, ARMARX_DEBUG);
+ {
+ viz::Layer layerFrames = arviz.layer("FramesIndividual");
+ visualizeFramesIndividual(layerFrames, frames);
+ layers.push_back(layerFrames);
+ }
+
// Commit layers.
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
index 66222e0cf8dc6cf6043e6db35ebb7ba00e0cab00..99321b656be9977a23096e3b9bdd9ed4ba62a101 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
+++ b/source/RobotAPI/libraries/armem_robot_state/server/common/Visu.h
@@ -71,6 +71,11 @@ namespace armarx::armem::server::robot_state
viz::Layer& layerFrames,
const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+ static
+ void visualizeFramesIndividual(
+ viz::Layer& layerFrames,
+ const std::unordered_map<std::string, std::vector<Eigen::Affine3f>>& frames);
+
private:
diff --git a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
index 53021dc7d88f9362a967f395b1381d07934f271e..1233be7dadb4a548efd9d7c8c38a797d4138c707 100644
--- a/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+++ b/source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
@@ -3,7 +3,6 @@
#include <filesystem>
#include <istream>
-
#include "ArmarXCore/core/time/Frequency.h"
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
@@ -11,13 +10,11 @@
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
-
namespace armarx::armem::server::robot_state::proprioception
{
RobotUnitReader::RobotUnitReader() = default;
-
void
RobotUnitReader::connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
@@ -48,7 +45,6 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
-
void
RobotUnitReader::run(float pollFrequency, Queue& dataBuffer)
{
@@ -56,14 +52,22 @@ namespace armarx::armem::server::robot_state::proprioception
while (task and not task->isStopped())
{
+ auto start = std::chrono::high_resolution_clock::now();
+
if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
// will lock a mutex
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t commitTimestamp [us]",
+ commit->timestamp.toMicroSecondsSinceEpoch());
+
dataBuffer.push(std::move(commit.value()));
}
-
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start);
if (debugObserver)
{
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t run [ms]",
+ duration.count() / 1000.f);
debugObserver->sendDebugObserverBatch();
}
@@ -71,13 +75,24 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
-
std::optional<RobotUnitData>
RobotUnitReader::fetchAndConvertLatestRobotUnitData()
{
ARMARX_CHECK_NOT_NULL(converterProprioception);
- const std::optional<RobotUnitDataStreaming::TimeStep> data = fetchLatestData();
+
+ std::optional<RobotUnitDataStreaming::TimeStep> data;
+ {
+ auto start = std::chrono::high_resolution_clock::now();
+ data = fetchLatestData();
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
+ std::chrono::high_resolution_clock::now() - start);
+ if (debugObserver)
+ {
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | t Fetch [ms]",
+ duration.count() / 1000.f);
+ }
+ }
if (not data.has_value())
{
return std::nullopt;
@@ -88,16 +103,16 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitData result;
- if(converterProprioception != nullptr)
+ if (converterProprioception != nullptr)
{
result.proprioception = converterProprioception->convert(data.value(), description);
}
- if(converterExteroception != nullptr)
+ if (converterExteroception != nullptr)
{
result.exteroception = converterExteroception->convert(data.value(), description);
}
-
+
result.timestamp = Time(Duration::MicroSeconds(data->timestampUSec));
auto stop = std::chrono::high_resolution_clock::now();
@@ -114,7 +129,6 @@ namespace armarx::armem::server::robot_state::proprioception
return result;
}
-
std::optional<RobotUnitDataStreaming::TimeStep>
RobotUnitReader::fetchLatestData()
{
@@ -122,6 +136,14 @@ namespace armarx::armem::server::robot_state::proprioception
if (debugObserver)
{
debugObserver->setDebugObserverDatafield("RobotUnitReader | Buffer Size", data.size());
+ if (data.size())
+ {
+ debugObserver->setDebugObserverDatafield("RobotUnitReader | RT Timestamp USec",
+ data.back().timestampUSec);
+ debugObserver->setDebugObserverDatafield(
+ "RobotUnitReader | RT TS Since Last Iteration",
+ data.back().timesSinceLastIterationUSec);
+ }
}
if (data.empty())
{