Skip to content
Snippets Groups Projects
Commit 4c48b8ef authored by Christoph Pohl's avatar Christoph Pohl
Browse files

Autoformatting

parent 0327cab6
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+ 364
− 357
View file @ 4c48b8ef
......@@ -427,8 +427,8 @@ namespace armarx::RobotUnitModule
continue; //do not add to result and skipp during processing
}
auto& descrEntr = descr.entries[valData.fields.at(i).name];
//*INDENT-OFF*
// clang-format off
//*INDENT-OFF*
// clang-format off
#define make_case(Type, TName) \
(typeid(Type) == *valData.fields.at(i).type) \
{ \
......@@ -494,7 +494,7 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard{rtLoggingMutex};
if (rtDataStreamingEntry.count(receiver) == 0u)
{
ARMARX_INFO << "stopDataStreaming called for a nonexistent log";
......@@ -755,441 +755,448 @@ namespace armarx::RobotUnitModule
}
// Process devices.
{// Sensors.
{ARMARX_TRACE;
durations.sens.start();
//sensors
for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
{
const SensorValueBase* val = data.sensors.at(idxDev);
//dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
{ // Sensors.
{
if (!rtLoggingEntries.empty())
ARMARX_TRACE;
durations.sens.start();
//sensors
for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
{
durations.sens_csv.start();
const auto str = val->getDataFieldAs<std::string>(idxField);
for (auto& [_, entry] : rtLoggingEntries)
const SensorValueBase* val = data.sensors.at(idxDev);
//dimensions of sensor value (e.g. vel, tor, f_x, f_y, ...)
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
++idxField)
{
if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
if (!rtLoggingEntries.empty())
{
entry->stream << ";" << str;
durations.sens_csv.start();
const auto str = val->getDataFieldAs<std::string>(idxField);
for (auto& [_, entry] : rtLoggingEntries)
{
if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
{
entry->stream << ";" << str;
}
}
durations.sens_csv.stop();
}
if (!rtDataStreamingEntry.empty())
{
durations.sens_stream.start();
for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
{
durations.sens_stream_elem.start();
rtStreamingEntry.processSens(*val, idxDev, idxField);
durations.sens_stream_elem.stop();
}
durations.sens_stream.stop();
}
}
durations.sens_csv.stop();
}
if (!rtDataStreamingEntry.empty())
durations.sens.stop();
}
// Controller.
{
durations.ctrl.start();
ARMARX_TRACE;
//joint controllers
for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
{
durations.sens_stream.start();
for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
const auto& vals = data.control.at(idxDev);
//control value (e.g. v_platform)
for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
durations.sens_stream_elem.start();
rtStreamingEntry.processSens(*val, idxDev, idxField);
durations.sens_stream_elem.stop();
const ControlTargetBase* val = vals.at(idxVal);
//dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields();
++idxField)
{
if (!rtLoggingEntries.empty())
{
durations.ctrl_csv.start();
std::string str;
val->getDataFieldAs(idxField, str); // expensive function
for (auto& [_, entry] : rtLoggingEntries)
{
if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(
idxField))
{
entry->stream << ";" << str;
}
}
durations.ctrl_csv.stop();
}
if (!rtDataStreamingEntry.empty())
{
durations.ctrl_stream.start();
for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
{
durations.ctrl_stream_elem.start();
rtStreamingEntry.processCtrl(*val, idxDev, idxVal, idxField);
durations.ctrl_stream_elem.stop();
}
durations.ctrl_stream.stop();
}
}
}
durations.sens_stream.stop();
}
durations.ctrl.stop();
}
}
durations.sens.stop();
}
} // namespace armarx::RobotUnitModule
// Controller.
{
durations.ctrl.start();
ARMARX_TRACE;
//joint controllers
for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
//finish processing
{
const auto& vals = data.control.at(idxDev);
//control value (e.g. v_platform)
for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
//store data to backlog
{
const ControlTargetBase* val = vals.at(idxVal);
//dimensions of control value (e.g. v_platform_x, v_platform_y, v_platform_rotate)
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++idxField)
if (rtLoggingBacklogEnabled)
{
if (!rtLoggingEntries.empty())
durations.backlog.start();
ARMARX_TRACE;
if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
durations.ctrl_csv.start();
std::string str;
val->getDataFieldAs(idxField, str); // expensive function
for (auto& [_, entry] : rtLoggingEntries)
{
if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
{
entry->stream << ";" << str;
}
}
durations.ctrl_csv.stop();
backlog.emplace_back(data, true); //true for minimal copy
}
if (!rtDataStreamingEntry.empty())
durations.backlog.stop();
}
}
//print + reset messages
{
durations.msg.start();
ARMARX_TRACE;
for (const ::armarx::detail::RtMessageLogEntryBase* ptr :
data.messages.getEntries())
{
if (!ptr)
{
durations.ctrl_stream.start();
for (auto& [_, rtStreamingEntry] : rtDataStreamingEntry)
{
durations.ctrl_stream_elem.start();
rtStreamingEntry.processCtrl(*val, idxDev, idxVal, idxField);
durations.ctrl_stream_elem.stop();
}
durations.ctrl_stream.stop();
break;
}
ptr->print(controlThreadId);
}
durations.msg.stop();
}
}
durations.ctrl.stop();
}
} // namespace armarx::RobotUnitModule
//finish processing
{
//store data to backlog
bool
Logging::MatchName(const std::string& pattern, const std::string& name)
{
if (rtLoggingBacklogEnabled)
ARMARX_TRACE;
if (pattern.empty())
{
durations.backlog.start();
ARMARX_TRACE;
if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
backlog.emplace_back(data, true); //true for minimal copy
}
durations.backlog.stop();
return false;
}
}
//print + reset messages
{
durations.msg.start();
ARMARX_TRACE;
for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
if (!std::regex_match(pattern, pattern_regex))
{
if (!ptr)
{
break;
}
ptr->print(controlThreadId);
throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
}
durations.msg.stop();
static const std::regex reg_dot{"[.]"};
static const std::regex reg_star{"[*]"};
const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
const std::regex key_regex{rpl2};
return std::regex_search(name, key_regex);
}
}
}
bool
Logging::MatchName(const std::string& pattern, const std::string& name)
{
ARMARX_TRACE;
if (pattern.empty())
{
return false;
}
static const std::regex pattern_regex{R"(^\^?[- ._*a-zA-Z0-9]+\$?$)"};
if (!std::regex_match(pattern, pattern_regex))
{
throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
}
static const std::regex reg_dot{"[.]"};
static const std::regex reg_star{"[*]"};
const std::string rpl1 = std::regex_replace(pattern, reg_dot, "\\.");
const std::string rpl2 = std::regex_replace(rpl1, reg_star, ".*");
const std::regex key_regex{rpl2};
return std::regex_search(name, key_regex);
}
void
Logging::_postOnInitRobotUnit()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
rtLoggingBacklogRetentionTime =
IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
rtLoggingBacklogMaxSize = getProperty<std::size_t>("RTLogging_MaxBacklogSize");
rtLoggingBacklogEnabled = getProperty<bool>("RTLogging_EnableBacklog");
getProperty(numberOfEntriesToLog, "RTLogging_LogLastNMessagesOnly");
ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
ARMARX_IMPORTANT << "Initializing RTLogging with the following parameters: " << VAROUT(rtLoggingTimestepMs)
<< VAROUT(messageBufferSize) << VAROUT(messageBufferMaxSize)
<< VAROUT(messageBufferNumberEntries) << VAROUT(messageBufferMaxNumberEntries)
<< VAROUT(rtLoggingBacklogRetentionTime) << VAROUT(rtLoggingBacklogMaxSize)
<< VAROUT(rtLoggingBacklogEnabled) << VAROUT(numberOfEntriesToLog);
}
void
Logging::_postFinishDeviceInitialization()
{
ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//init buffer
void
Logging::_postOnInitRobotUnit()
{
ARMARX_TRACE;
std::size_t ctrlThreadPeriodUs =
static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
const auto bufferSize =
_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
nBuffers,
_module<Devices>().getControlDevices(),
_module<Devices>().getSensorDevices(),
messageBufferSize,
messageBufferNumberEntries,
messageBufferMaxSize,
messageBufferMaxNumberEntries);
ARMARX_INFO << "RTLogging activated. Using " << nBuffers << " buffers "
<< "(buffersize = " << bufferSize << " bytes)";
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
rtLoggingTimestepMs = getProperty<std::size_t>("RTLogging_PeriodMs");
ARMARX_CHECK_LESS(0, rtLoggingTimestepMs) << "The property RTLoggingPeriodMs must not be 0";
messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
rtLoggingBacklogRetentionTime =
IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
rtLoggingBacklogMaxSize = getProperty<std::size_t>("RTLogging_MaxBacklogSize");
rtLoggingBacklogEnabled = getProperty<bool>("RTLogging_EnableBacklog");
getProperty(numberOfEntriesToLog, "RTLogging_LogLastNMessagesOnly");
ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
ARMARX_IMPORTANT << "Initializing RTLogging with the following parameters: "
<< VAROUT(rtLoggingTimestepMs) << VAROUT(messageBufferSize)
<< VAROUT(messageBufferMaxSize) << VAROUT(messageBufferNumberEntries)
<< VAROUT(messageBufferMaxNumberEntries)
<< VAROUT(rtLoggingBacklogRetentionTime) << VAROUT(rtLoggingBacklogMaxSize)
<< VAROUT(rtLoggingBacklogEnabled) << VAROUT(numberOfEntriesToLog);
}
//init logging names + field types
void
Logging::_postFinishDeviceInitialization()
{
ARMARX_TRACE;
const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
//init buffer
{
ARMARX_TRACE;
std::size_t ctrlThreadPeriodUs =
static_cast<std::size_t>(getControlThreadTargetPeriod().toMicroSeconds());
std::size_t logThreadPeriodUs = rtLoggingTimestepMs * 1000;
std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
const auto bufferSize =
_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().initialize(
nBuffers,
_module<Devices>().getControlDevices(),
_module<Devices>().getSensorDevices(),
messageBufferSize,
messageBufferNumberEntries,
messageBufferMaxSize,
messageBufferMaxNumberEntries);
ARMARX_INFO << "RTLogging activated. Using " << nBuffers << " buffers "
<< "(buffersize = " << bufferSize << " bytes)";
}
//init logging names + field types
{
ValueMetaData data;
const auto names = val->getDataFieldNames();
data.fields.resize(names.size());
ARMARX_TRACE;
const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
{
ValueMetaData data;
const auto names = val->getDataFieldNames();
data.fields.resize(names.size());
for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++fieldIdx)
for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++fieldIdx)
{
std::string const& fieldName = names[fieldIdx];
data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
}
return data;
};
//sensorDevices
controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
for (const auto& cd : _module<Devices>().getControlDevices().values())
{
std::string const& fieldName = names[fieldIdx];
data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
ARMARX_TRACE;
controlDeviceValueMetaData.emplace_back();
auto& dataForDev = controlDeviceValueMetaData.back();
dataForDev.reserve(cd->getJointControllers().size());
for (auto jointC : cd->getJointControllers())
{
dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
"ctrl." + cd->getDeviceName() + "." +
jointC->getControlMode()));
}
}
return data;
};
//sensorDevices
controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
for (const auto& cd : _module<Devices>().getControlDevices().values())
{
ARMARX_TRACE;
controlDeviceValueMetaData.emplace_back();
auto& dataForDev = controlDeviceValueMetaData.back();
dataForDev.reserve(cd->getJointControllers().size());
for (auto jointC : cd->getJointControllers())
//sensorDevices
sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
for (const auto& sd : _module<Devices>().getSensorDevices().values())
{
dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
"ctrl." + cd->getDeviceName() + "." +
jointC->getControlMode()));
ARMARX_TRACE;
sensorDeviceValueMetaData.emplace_back(
makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
}
}
//sensorDevices
sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
for (const auto& sd : _module<Devices>().getSensorDevices().values())
//start logging thread is done in rtinit
//maybe add the default log
{
ARMARX_TRACE;
sensorDeviceValueMetaData.emplace_back(
makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
if (!loggingpath.empty())
{
defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
}
}
}
//start logging thread is done in rtinit
//maybe add the default log
void
Logging::DataStreamingEntry::clearResult()
{
ARMARX_TRACE;
const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
if (!loggingpath.empty())
for (auto& e : result)
{
defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
entryBuffer.emplace_back(std::move(e));
}
result.clear();
}
}
void
Logging::DataStreamingEntry::clearResult()
{
ARMARX_TRACE;
for (auto& e : result)
RobotUnitDataStreaming::TimeStep
Logging::DataStreamingEntry::allocateResultElement() const
{
entryBuffer.emplace_back(std::move(e));
ARMARX_TRACE;
RobotUnitDataStreaming::TimeStep data;
data.bools.resize(numBools);
data.bytes.resize(numBytes);
data.shorts.resize(numShorts);
data.ints.resize(numInts);
data.longs.resize(numLongs);
data.floats.resize(numFloats);
data.doubles.resize(numDoubles);
return data;
}
result.clear();
}
RobotUnitDataStreaming::TimeStep
Logging::DataStreamingEntry::allocateResultElement() const
{
ARMARX_TRACE;
RobotUnitDataStreaming::TimeStep data;
data.bools.resize(numBools);
data.bytes.resize(numBytes);
data.shorts.resize(numShorts);
data.ints.resize(numInts);
data.longs.resize(numLongs);
data.floats.resize(numFloats);
data.doubles.resize(numDoubles);
return data;
}
RobotUnitDataStreaming::TimeStep
Logging::DataStreamingEntry::getResultElement()
{
ARMARX_TRACE;
if (entryBuffer.empty())
RobotUnitDataStreaming::TimeStep
Logging::DataStreamingEntry::getResultElement()
{
return allocateResultElement();
ARMARX_TRACE;
if (entryBuffer.empty())
{
return allocateResultElement();
}
auto e = std::move(entryBuffer.back());
entryBuffer.pop_back();
return e;
}
auto e = std::move(entryBuffer.back());
entryBuffer.pop_back();
return e;
}
void
Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
{
ARMARX_TRACE;
if (stopStreaming)
void
Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
{
return;
}
ARMARX_TRACE;
if (stopStreaming)
{
return;
}
auto& data = result.emplace_back(getResultElement());
auto& data = result.emplace_back(getResultElement());
data.iterationId = e.iteration;
data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
}
data.iterationId = e.iteration;
data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
}
void
WriteTo(const auto& dentr,
const Logging::DataStreamingEntry::OutVal& out,
const auto& val,
std::size_t fidx,
auto& data)
{
ARMARX_TRACE;
using enum_t = Logging::DataStreamingEntry::ValueT;
try
void
WriteTo(const auto& dentr,
const Logging::DataStreamingEntry::OutVal& out,
const auto& val,
std::size_t fidx,
auto& data)
{
ARMARX_TRACE;
switch (out.value)
using enum_t = Logging::DataStreamingEntry::ValueT;
try
{
case enum_t::Bool:
bool b;
val.getDataFieldAs(fidx, b);
data.bools.at(out.idx) = b;
return;
case enum_t::Byte:
val.getDataFieldAs(fidx, data.bytes.at(out.idx));
return;
case enum_t::Short:
val.getDataFieldAs(fidx, data.shorts.at(out.idx));
return;
case enum_t::Int:
val.getDataFieldAs(fidx, data.ints.at(out.idx));
return;
case enum_t::Long:
val.getDataFieldAs(fidx, data.longs.at(out.idx));
return;
case enum_t::Float:
val.getDataFieldAs(fidx, data.floats.at(out.idx));
return;
case enum_t::Double:
val.getDataFieldAs(fidx, data.doubles.at(out.idx));
return;
case enum_t::Skipped:
return;
ARMARX_TRACE;
switch (out.value)
{
case enum_t::Bool:
bool b;
val.getDataFieldAs(fidx, b);
data.bools.at(out.idx) = b;
return;
case enum_t::Byte:
val.getDataFieldAs(fidx, data.bytes.at(out.idx));
return;
case enum_t::Short:
val.getDataFieldAs(fidx, data.shorts.at(out.idx));
return;
case enum_t::Int:
val.getDataFieldAs(fidx, data.ints.at(out.idx));
return;
case enum_t::Long:
val.getDataFieldAs(fidx, data.longs.at(out.idx));
return;
case enum_t::Float:
val.getDataFieldAs(fidx, data.floats.at(out.idx));
return;
case enum_t::Double:
val.getDataFieldAs(fidx, data.doubles.at(out.idx));
return;
case enum_t::Skipped:
return;
}
}
catch (...)
{
ARMARX_ERROR << GetHandledExceptionString() << "\ntype " << static_cast<int>(out.value)
<< "\n"
<< VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools) << "\n"
<< VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes) << "\n"
<< VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts) << "\n"
<< VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts) << "\n"
<< VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs) << "\n"
<< VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats) << "\n"
<< VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
throw;
}
}
catch (...)
{
ARMARX_ERROR << GetHandledExceptionString() << "\ntype " << static_cast<int>(out.value)
<< "\n"
<< VAROUT(data.bools.size()) << " " << VAROUT(dentr.numBools) << "\n"
<< VAROUT(data.bytes.size()) << " " << VAROUT(dentr.numBytes) << "\n"
<< VAROUT(data.shorts.size()) << " " << VAROUT(dentr.numShorts) << "\n"
<< VAROUT(data.ints.size()) << " " << VAROUT(dentr.numInts) << "\n"
<< VAROUT(data.longs.size()) << " " << VAROUT(dentr.numLongs) << "\n"
<< VAROUT(data.floats.size()) << " " << VAROUT(dentr.numFloats) << "\n"
<< VAROUT(data.doubles.size()) << " " << VAROUT(dentr.numDoubles);
throw;
}
}
void
Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
std::size_t didx,
std::size_t vidx,
std::size_t fidx)
{
ARMARX_TRACE;
if (stopStreaming)
void
Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
std::size_t didx,
std::size_t vidx,
std::size_t fidx)
{
return;
ARMARX_TRACE;
if (stopStreaming)
{
return;
}
auto& data = result.back();
const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
WriteTo(*this, o, val, fidx, data);
}
auto& data = result.back();
const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
WriteTo(*this, o, val, fidx, data);
}
void
Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
std::size_t didx,
std::size_t fidx)
{
ARMARX_TRACE;
if (stopStreaming)
void
Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
std::size_t didx,
std::size_t fidx)
{
return;
ARMARX_TRACE;
if (stopStreaming)
{
return;
}
auto& data = result.back();
const OutVal& o = sensDevs.at(didx).at(fidx);
WriteTo(*this, o, val, fidx, data);
}
auto& data = result.back();
const OutVal& o = sensDevs.at(didx).at(fidx);
WriteTo(*this, o, val, fidx, data);
}
void
Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r, std::uint64_t msgId)
{
ARMARX_TRACE;
const auto start_send = armarx::rtNow();
const auto num_timesteps = result.size();
updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
const auto start_clear = armarx::rtNow();
clearResult();
const auto end = armarx::rtNow();
ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
<< "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms ("
<< num_timesteps << " timesteps)"
<< "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
//now execute all ready callbacks
std::size_t i = 0;
for (; i < updateCalls.size(); ++i)
void
Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r,
std::uint64_t msgId)
{
try
ARMARX_TRACE;
const auto start_send = armarx::rtNow();
const auto num_timesteps = result.size();
updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
const auto start_clear = armarx::rtNow();
clearResult();
const auto end = armarx::rtNow();
ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
<< "\n update " << (start_clear - start_send).toMilliSecondsDouble()
<< "ms (" << num_timesteps << " timesteps)"
<< "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
//now execute all ready callbacks
std::size_t i = 0;
for (; i < updateCalls.size(); ++i)
{
if (!updateCalls.at(i)->isCompleted())
try
{
break;
if (!updateCalls.at(i)->isCompleted())
{
break;
}
r->end_update(updateCalls.at(i));
connectionFailures = 0;
}
r->end_update(updateCalls.at(i));
connectionFailures = 0;
}
catch (...)
{
ARMARX_TRACE;
++connectionFailures;
if (connectionFailures > rtStreamMaxClientErrors)
catch (...)
{
stopStreaming = true;
ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
<< connectionFailures << " iterations! Removing receiver";
updateCalls.clear();
break;
ARMARX_TRACE;
++connectionFailures;
if (connectionFailures > rtStreamMaxClientErrors)
{
stopStreaming = true;
ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
<< connectionFailures << " iterations! Removing receiver";
updateCalls.clear();
break;
}
}
}
if (!updateCalls.empty())
{
updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
}
}
if (!updateCalls.empty())
{
updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
}
}
} // namespace armarx::RobotUnitModule
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