Skip to content
Snippets Groups Projects
Commit ec2d0593 authored by armar-user's avatar armar-user
Browse files

reduced verbosity

parent 915fcf42
No related branches found
No related tags found
1 merge request!369Updates/a6u1/2023 07 26
This commit is part of merge request !369. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.cpp
+ 50
− 36
View file @ ec2d0593
......@@ -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
source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+ 33
− 11
View file @ ec2d0593
......@@ -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())
{
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment