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_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())
{