diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 725bb380ed3638ac9ad71102375c0c4a4a0c8f5d..b12127e4d8541fbf332b249988658288ea05ea29 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -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