diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 0f45fe1f7bbd22ff33afc7859f595ecdc952ab60..3e50e1069a5ff136d8a4eba9df1c1d982ebccc2e 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -43,25 +43,31 @@ namespace armarx::RobotUnitModule::details
{
t = IceUtil::Time::seconds(0);
}
+
IceUtil::Time t;
+
void
start()
{
t -= IceUtil::Time::now();
++n;
}
+
void
stop()
{
t += IceUtil::Time::now();
}
+
double
ms() const
{
return t.toMilliSecondsDouble();
}
+
std::size_t n = 0;
};
+
DurationsEntry header;
DurationsEntry header_csv;
DurationsEntry header_stream;
@@ -389,7 +395,7 @@ namespace armarx::RobotUnitModule
DataStreamingEntry& streamingEntry = rtDataStreamingEntry[receiver];
getProperty(streamingEntry.rtStreamMaxClientErrors,
"RTLogging_StreamingDataMaxClientConnectionFailures");
-
+
getProperty(logAllEntries, "RTLogging_LogAllMessages");
ARMARX_INFO << "start data streaming to " << receiver->ice_getIdentity().name
@@ -421,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];
-//formatter failes here!
//*INDENT-OFF*
+// clang-format off
#define make_case(Type, TName) \
(typeid(Type) == *valData.fields.at(i).type) \
{ \
@@ -433,27 +439,26 @@ namespace armarx::RobotUnitModule
++streamingEntry.num##TName##s; \
}
if make_case (bool, Bool)
- else if make_case (Ice::Byte, Byte) else if make_case (Ice::Short, Short) else if make_case (Ice::Int, Int) else if make_case (
- Ice::Long,
- Long) else if make_case (Ice::Float,
- Float) else if make_case (Ice::Double,
- Double) else if make_case (std::
- uint16_t,
- Long) else if make_case (std::
- uint32_t,
- Long) else
- {
- ARMARX_CHECK_EXPRESSION(false)
- << "This code sould be unreachable! "
- "The type of "
- << valData.fields.at(i).name << " is not handled correctly!";
- }
+ else if make_case (Ice::Byte, Byte)
+ else if make_case (Ice::Short, Short)
+ else if make_case (Ice::Int, Int)
+ else if make_case (Ice::Long, Long)
+ else if make_case (Ice::Float, Float)
+ else if make_case (Ice::Double, Double)
+ else if make_case (std::uint16_t, Long)
+ else if make_case (std::uint32_t, Long)
+ else {
+ ARMARX_CHECK_EXPRESSION(false)
+ << "This code sould be unreachable! "
+ "The type of "
+ << valData.fields.at(i).name << " is not handled correctly!";
+ }
#undef make_case
- //*INDENT-ON*
}
return result;
};
-
+ //*INDENT-ON*
+ // clang-format on
//get logged sensor device values
{
ARMARX_TRACE;
@@ -560,10 +565,14 @@ namespace armarx::RobotUnitModule
//remove backlog entries
const auto start_time_remove_backlog_entries = IceUtil::Time::now();
{
- while (!backlog.empty() &&
- backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now)
+ if (rtLoggingBacklogEnabled)
{
- backlog.pop_front();
+ while (!backlog.empty() &&
+ (backlog.front().writeTimestamp + rtLoggingBacklogRetentionTime < now or
+ backlog.size() > rtLoggingBacklogMaxSize))
+ {
+ backlog.pop_front();
+ }
}
}
//log all
@@ -579,7 +588,7 @@ namespace armarx::RobotUnitModule
}
- if(logAllEntries)
+ if (logAllEntries)
{
_module<ControlThreadDataBuffer>()
.getControlThreadOutputBuffer()
@@ -589,7 +598,9 @@ namespace armarx::RobotUnitModule
ARMARX_TRACE;
doLogging(dlogdurs, now, data, i, num);
});
- }else { // only log newest entry
+ }
+ else
+ { // only log newest entry
_module<ControlThreadDataBuffer>()
.getControlThreadOutputBuffer()
.forLatestLoggingEntry(
@@ -599,7 +610,6 @@ namespace armarx::RobotUnitModule
doLogging(dlogdurs, now, data, i, num);
});
}
-
}
ARMARX_DEBUG << ::deactivateSpam() << "the last " << backlog.size()
<< " iterations are stored";
@@ -657,27 +667,45 @@ namespace armarx::RobotUnitModule
}
// clang-format off
const auto end_time = IceUtil::Time::now();
- const auto time_total = (end_time - now).toMilliSecondsDouble();
+ const auto time_total = (end_time - now).toMilliSecondsDouble();
ARMARX_DEBUG << deactivateSpam(1)
<< "rtlogging time required: " << time_total << "ms\n"
- << " time_remove_backlog_entries " << (start_time_log_all - start_time_remove_backlog_entries).toMilliSecondsDouble() << "ms\n"
- << " time_log_all " << (start_time_flush_all_files - start_time_log_all).toMilliSecondsDouble() << "ms\n"
- << " header " << dlogdurs.header.ms() << "ms\t(" << dlogdurs.header.n << " calls)\n"
- << " csv " << dlogdurs.header_csv.ms() << "ms\t(" << dlogdurs.header_csv.n << " calls)\n"
- << " stream " << dlogdurs.header_stream.ms() << "ms\t(" << dlogdurs.header_stream.n << " calls)\n"
- << " sens " << dlogdurs.sens.ms() << "ms\t(" << dlogdurs.sens.n << " calls)\n"
- << " csv " << dlogdurs.sens_csv.ms() << "ms\t(" << dlogdurs.sens_csv.n << " calls)\n"
- << " stream " << dlogdurs.sens_stream.ms() << "ms\t(" << dlogdurs.sens_stream.n << " calls)\n"
- << " per elem " << dlogdurs.sens_stream_elem.ms() << "ms\t(" << dlogdurs.sens_stream_elem.n << " calls)\n"
- << " ctrl " << dlogdurs.ctrl.ms() << "ms\t(" << dlogdurs.ctrl.n << " calls)\n"
- << " csv " << dlogdurs.ctrl_csv.ms() << "ms\t(" << dlogdurs.ctrl_csv.n << " calls)\n"
- << " stream " << dlogdurs.ctrl_stream.ms() << "ms\t(" << dlogdurs.ctrl_stream.n << " calls)\n"
- << " per elem " << dlogdurs.ctrl_stream_elem.ms() << "ms\t(" << dlogdurs.ctrl_stream_elem.n << " calls)\n"
- << " backlog " << dlogdurs.backlog.ms() << "ms\t(" << dlogdurs.backlog.n << " calls)\n"
- << " msg " << dlogdurs.msg.ms() << "ms\t(" << dlogdurs.msg.n << " calls)\n"
- << " time_flush_all_files " << (start_time_remove_entries - start_time_flush_all_files).toMilliSecondsDouble() << "ms\n"
- << " time_remove_entries " << (start_time_data_streaming - start_time_remove_entries).toMilliSecondsDouble() << "ms\n"
- << " time_data_streaming " << (end_time - start_time_data_streaming).toMilliSecondsDouble() << "ms\n";
+ << " time_remove_backlog_entries "
+ << (start_time_log_all - start_time_remove_backlog_entries).toMilliSecondsDouble() << "ms\n"
+ << " time_log_all "
+ << (start_time_flush_all_files - start_time_log_all).toMilliSecondsDouble() << "ms\n"
+ << " header " << dlogdurs.header.ms() << "ms\t(" << dlogdurs.header.n
+ << " calls)\n"
+ << " csv " << dlogdurs.header_csv.ms() << "ms\t("
+ << dlogdurs.header_csv.n << " calls)\n"
+ << " stream " << dlogdurs.header_stream.ms() << "ms\t("
+ << dlogdurs.header_stream.n << " calls)\n"
+ << " sens " << dlogdurs.sens.ms() << "ms\t(" << dlogdurs.sens.n
+ << " calls)\n"
+ << " csv " << dlogdurs.sens_csv.ms() << "ms\t(" << dlogdurs.sens_csv.n
+ << " calls)\n"
+ << " stream " << dlogdurs.sens_stream.ms() << "ms\t("
+ << dlogdurs.sens_stream.n << " calls)\n"
+ << " per elem " << dlogdurs.sens_stream_elem.ms() << "ms\t("
+ << dlogdurs.sens_stream_elem.n << " calls)\n"
+ << " ctrl " << dlogdurs.ctrl.ms() << "ms\t(" << dlogdurs.ctrl.n
+ << " calls)\n"
+ << " csv " << dlogdurs.ctrl_csv.ms() << "ms\t(" << dlogdurs.ctrl_csv.n
+ << " calls)\n"
+ << " stream " << dlogdurs.ctrl_stream.ms() << "ms\t("
+ << dlogdurs.ctrl_stream.n << " calls)\n"
+ << " per elem " << dlogdurs.ctrl_stream_elem.ms() << "ms\t("
+ << dlogdurs.ctrl_stream_elem.n << " calls)\n"
+ << " backlog " << dlogdurs.backlog.ms() << "ms\t(" << dlogdurs.backlog.n
+ << " calls)\n"
+ << " msg " << dlogdurs.msg.ms() << "ms\t(" << dlogdurs.msg.n
+ << " calls)\n"
+ << " time_flush_all_files "
+ << (start_time_remove_entries - start_time_flush_all_files).toMilliSecondsDouble() << "ms\n"
+ << " time_remove_entries "
+ << (start_time_data_streaming - start_time_remove_entries).toMilliSecondsDouble() << "ms\n"
+ << " time_data_streaming "
+ << (end_time - start_time_data_streaming).toMilliSecondsDouble() << "ms\n";
// clang-format on
}
@@ -723,432 +751,434 @@ namespace armarx::RobotUnitModule
}
// Process devices.
+ {// Sensors.
+ {ARMARX_TRACE;
+ durations.sens.start();
+ //sensors
+ for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
{
- // Sensors.
+ 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)
{
- ARMARX_TRACE;
- durations.sens.start();
- //sensors
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++idxDev)
+ if (!rtLoggingEntries.empty())
{
- 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)
+ durations.sens_csv.start();
+ const auto str = val->getDataFieldAs<std::string>(idxField);
+ for (auto& [_, entry] : rtLoggingEntries)
{
- if (!rtLoggingEntries.empty())
- {
- 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())
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
{
- durations.sens_stream.start();
- for (auto& [_, data] : rtDataStreamingEntry)
- {
- durations.sens_stream_elem.start();
- data.processSens(*val, idxDev, idxField);
- durations.sens_stream_elem.stop();
- }
- durations.sens_stream.stop();
+ 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.stop();
}
+ }
+ durations.sens.stop();
+ }
- // Controller.
+ // Controller.
+ {
+ durations.ctrl.start();
+ ARMARX_TRACE;
+ //joint controllers
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
+ {
+ const auto& vals = data.control.at(idxDev);
+ //control value (e.g. v_platform)
+ for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
{
- durations.ctrl.start();
- ARMARX_TRACE;
- //joint controllers
- for (std::size_t idxDev = 0; idxDev < data.control.size(); ++idxDev)
+ 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)
{
- const auto& vals = data.control.at(idxDev);
- //control value (e.g. v_platform)
- for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
+ if (!rtLoggingEntries.empty())
{
- 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)
+ durations.ctrl_csv.start();
+ std::string str;
+ val->getDataFieldAs(idxField, str); // expensive function
+ for (auto& [_, entry] : rtLoggingEntries)
{
- 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())
+ if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
{
- durations.ctrl_stream.start();
- for (auto& [_, data] : rtDataStreamingEntry)
- {
- durations.ctrl_stream_elem.start();
- data.processCtrl(*val, idxDev, idxVal, idxField);
- durations.ctrl_stream_elem.stop();
- }
- durations.ctrl_stream.stop();
+ 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.ctrl.stop();
}
}
- //finish processing
+ durations.ctrl.stop();
+ }
+} // namespace armarx::RobotUnitModule
+
+//finish processing
+{
+ //store data to backlog
+ {
+ if (rtLoggingBacklogEnabled)
{
- //store data to backlog
- {
- durations.backlog.start();
- ARMARX_TRACE;
- if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
- {
- backlog.emplace_back(data, true); //true for minimal copy
- }
- durations.backlog.stop();
- }
- //print + reset messages
+ durations.backlog.start();
+ ARMARX_TRACE;
+ if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
- durations.msg.start();
- ARMARX_TRACE;
- for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
- {
- if (!ptr)
- {
- break;
- }
- ptr->print(controlThreadId);
- }
- durations.msg.stop();
+ backlog.emplace_back(data, true); //true for minimal copy
}
+ durations.backlog.stop();
}
}
-
- bool
- Logging::MatchName(const std::string& pattern, const std::string& name)
+ //print + reset messages
{
+ durations.msg.start();
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))
+ for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
{
- throw InvalidArgumentException{"Pattern '" + pattern + "' is invalid"};
+ if (!ptr)
+ {
+ break;
+ }
+ ptr->print(controlThreadId);
}
- 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);
+ durations.msg.stop();
}
+}
+}
- void
- Logging::_postOnInitRobotUnit()
+bool
+Logging::MatchName(const std::string& pattern, const std::string& name)
+{
+ ARMARX_TRACE;
+ if (pattern.empty())
{
- 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";
+ 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);
+}
- messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
- messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
- ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
+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";
- messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
- messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
- ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
+ messageBufferSize = getProperty<std::size_t>("RTLogging_MessageBufferSize");
+ messageBufferMaxSize = getProperty<std::size_t>("RTLogging_MaxMessageBufferSize");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferSize, messageBufferMaxSize);
- rtLoggingBacklogRetentionTime =
- IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
-
- ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
- }
+ messageBufferNumberEntries = getProperty<std::size_t>("RTLogging_MessageNumber");
+ messageBufferMaxNumberEntries = getProperty<std::size_t>("RTLogging_MaxMessageNumber");
+ ARMARX_CHECK_LESS_EQUAL(messageBufferNumberEntries, messageBufferMaxNumberEntries);
- void
- Logging::_postFinishDeviceInitialization()
+ rtLoggingBacklogRetentionTime =
+ IceUtil::Time::milliSeconds(getProperty<std::size_t>("RTLogging_KeepIterationsForMs"));
+ rtLoggingBacklogMaxSize = getProperty<std::size_t>("RTLogging_MaxBacklogSize");
+ rtLoggingBacklogEnabled = getProperty<bool>("RTLogging_EnableBacklog");
+ ARMARX_CHECK_GREATER(getControlThreadTargetPeriod().toMicroSeconds(), 0);
+}
+
+void
+Logging::_postFinishDeviceInitialization()
+{
+ ARMARX_TRACE;
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ //init buffer
{
ARMARX_TRACE;
- 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
- {
- ARMARX_TRACE;
- const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
- {
- ValueMetaData data;
- const auto names = val->getDataFieldNames();
- data.fields.resize(names.size());
+ 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;
- 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;
- };
+ 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
+ {
+ ARMARX_TRACE;
+ const auto makeValueMetaData = [&](auto* val, const std::string& namePre)
+ {
+ ValueMetaData data;
+ const auto names = val->getDataFieldNames();
+ data.fields.resize(names.size());
- //sensorDevices
- controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
- for (const auto& cd : _module<Devices>().getControlDevices().values())
+ for (std::size_t fieldIdx = 0; fieldIdx < names.size(); ++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()));
- }
+ std::string const& fieldName = names[fieldIdx];
+ data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
+ data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
}
- //sensorDevices
- sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
- for (const auto& sd : _module<Devices>().getSensorDevices().values())
+ 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())
{
- ARMARX_TRACE;
- sensorDeviceValueMetaData.emplace_back(
- makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
+ dataForDev.emplace_back(makeValueMetaData(jointC->getControlTarget(),
+ "ctrl." + cd->getDeviceName() + "." +
+ jointC->getControlMode()));
}
}
- //start logging thread is done in rtinit
- //maybe add the default log
+ //sensorDevices
+ sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
+ for (const auto& sd : _module<Devices>().getSensorDevices().values())
{
ARMARX_TRACE;
- const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
- if (!loggingpath.empty())
- {
- defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
- }
+ sensorDeviceValueMetaData.emplace_back(
+ makeValueMetaData(sd->getSensorValue(), "sens." + sd->getDeviceName()));
}
}
-
- void
- Logging::DataStreamingEntry::clearResult()
+ //start logging thread is done in rtinit
+ //maybe add the default log
{
ARMARX_TRACE;
- for (auto& e : result)
+ const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
+ if (!loggingpath.empty())
{
- entryBuffer.emplace_back(std::move(e));
+ defaultLogHandle = startRtLogging(loggingpath, getLoggingNames());
}
- result.clear();
}
+}
- RobotUnitDataStreaming::TimeStep
- Logging::DataStreamingEntry::allocateResultElement() const
+void
+Logging::DataStreamingEntry::clearResult()
+{
+ ARMARX_TRACE;
+ for (auto& e : result)
{
- 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;
+ entryBuffer.emplace_back(std::move(e));
}
+ result.clear();
+}
- RobotUnitDataStreaming::TimeStep
- Logging::DataStreamingEntry::getResultElement()
+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())
{
- ARMARX_TRACE;
- if (entryBuffer.empty())
- {
- return allocateResultElement();
- }
- auto e = std::move(entryBuffer.back());
- entryBuffer.pop_back();
- return e;
+ return allocateResultElement();
}
+ auto e = std::move(entryBuffer.back());
+ entryBuffer.pop_back();
+ return e;
+}
- void
- Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
+void
+Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
+{
+ ARMARX_TRACE;
+ if (stopStreaming)
{
- ARMARX_TRACE;
- if (stopStreaming)
- {
- return;
- }
-
- auto& data = result.emplace_back(getResultElement());
-
- data.iterationId = e.iteration;
- data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
- data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
+ return;
}
- void
- WriteTo(const auto& dentr,
- const Logging::DataStreamingEntry::OutVal& out,
- const auto& val,
- std::size_t fidx,
- auto& data)
+ auto& data = result.emplace_back(getResultElement());
+
+ 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
{
ARMARX_TRACE;
- using enum_t = Logging::DataStreamingEntry::ValueT;
- try
- {
- 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 (...)
+ switch (out.value)
{
- 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;
+ 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;
+ }
+}
- void
- Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
- std::size_t didx,
- std::size_t vidx,
- std::size_t fidx)
+void
+Logging::DataStreamingEntry::processCtrl(const ControlTargetBase& val,
+ std::size_t didx,
+ std::size_t vidx,
+ std::size_t fidx)
+{
+ ARMARX_TRACE;
+ if (stopStreaming)
{
- 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);
+ return;
}
+ 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)
+void
+Logging::DataStreamingEntry::processSens(const SensorValueBase& val,
+ std::size_t didx,
+ std::size_t fidx)
+{
+ ARMARX_TRACE;
+ if (stopStreaming)
{
- ARMARX_TRACE;
- if (stopStreaming)
- {
- return;
- }
- auto& data = result.back();
- const OutVal& o = sensDevs.at(didx).at(fidx);
- WriteTo(*this, o, val, fidx, data);
+ return;
}
+ 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)
+void
+Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r, std::uint64_t msgId)
+{
+ ARMARX_TRACE;
+ const auto start_send = IceUtil::Time::now();
+ updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
+ const auto start_clear = IceUtil::Time::now();
+ clearResult();
+ const auto end = IceUtil::Time::now();
+ ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
+ << "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms ("
+ << result.size() << " timesteps)"
+ << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
+
+ //now execute all ready callbacks
+ std::size_t i = 0;
+ for (; i < updateCalls.size(); ++i)
{
- ARMARX_TRACE;
- const auto start_send = IceUtil::Time::now();
- updateCalls.emplace_back(r->begin_update(result, static_cast<Ice::Long>(msgId)));
- const auto start_clear = IceUtil::Time::now();
- clearResult();
- const auto end = IceUtil::Time::now();
- ARMARX_DEBUG_S << "Logging::DataStreamingEntry::send"
- << "\n update " << (start_clear - start_send).toMilliSecondsDouble() << "ms ("
- << result.size() << " timesteps)"
- << "\n clear " << (end - start_clear).toMilliSecondsDouble() << "ms";
-
- //now execute all ready callbacks
- std::size_t i = 0;
- for (; i < updateCalls.size(); ++i)
+ try
{
- try
- {
- if (!updateCalls.at(i)->isCompleted())
- {
- break;
- }
- r->end_update(updateCalls.at(i));
- connectionFailures = 0;
- }
- catch (...)
+ if (!updateCalls.at(i)->isCompleted())
{
- ARMARX_TRACE;
- ++connectionFailures;
- if (connectionFailures > rtStreamMaxClientErrors)
- {
- stopStreaming = true;
- ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
- << connectionFailures << " iterations! Removing receiver";
- updateCalls.clear();
- break;
- }
+ break;
}
+ r->end_update(updateCalls.at(i));
+ connectionFailures = 0;
}
- if (!updateCalls.empty())
+ catch (...)
{
- updateCalls.erase(updateCalls.begin(), updateCalls.begin() + i);
+ 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);
+ }
+}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index bb6a625e3484fc843f4d27224a5801d416d91858..7529c75fd0182aa30c7c64ff994df15127b29009 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -71,10 +71,18 @@ namespace armarx::RobotUnitModule
"RTLogging_MaxMessageBufferSize", 16 * 1024 * 1024,
"Max number of bytes that can be logged in the control thread");
+ defineOptionalProperty<bool>(
+ "RTLogging_EnableBacklog", true,
+ "Enable/Disable the backlog (SensorValues, ControlTargets, Messages) that is kept"
+ "and can be dumped in case of an error.");
defineOptionalProperty<std::size_t>(
"RTLogging_KeepIterationsForMs", 60 * 1000,
"All logging data (SensorValues, ControlTargets, Messages) is kept for this duration "
"and can be dumped in case of an error.");
+ defineOptionalProperty<std::size_t>(
+ "RTLogging_MaxBacklogSize", 5000,
+ "Maximum size of the backlog (SensorValues, ControlTargets, Messages) that is kept"
+ "and can be dumped in case of an error.");
defineOptionalProperty<std::size_t>(
"RTLogging_StreamingDataMaxClientConnectionFailures", 10,
@@ -301,8 +309,12 @@ namespace armarx::RobotUnitModule
std::size_t messageBufferMaxNumberEntries {0};
/// @brief The logging thread's period
std::size_t rtLoggingTimestepMs {0};
- /// @brief The time an entry shold remain in the backlog.
+ /// @brief The time an entry should remain in the backlog.
IceUtil::Time rtLoggingBacklogRetentionTime;
+ /// @brief The maximum number of entries in the backlog.
+ std::size_t rtLoggingBacklogMaxSize;
+ /// @brief Enable/disable the backlog.
+ bool rtLoggingBacklogEnabled;
/// @brief
bool logAllEntries = true;
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
index c07f2d42c7d1d17def0b67a8c4866fe21384284e..243950a7c5a15f376f1f5c7b09b8b99fac0ea2d2 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModulePublisher.cpp
@@ -31,6 +31,9 @@
#include <RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleControlThreadDataBuffer.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+#include "ArmarXCore/core/logging/Logging.h"
+#include "ArmarXCore/core/util/StringHelpers.h"
#include <ArmarXCore/core/ArmarXObjectScheduler.h>
namespace armarx::RobotUnitModule
@@ -312,6 +315,7 @@ namespace armarx::RobotUnitModule
SensorDeviceStatus status;
status.deviceName = DevicesAttorneyForPublisher::GetSensorDeviceName(this, sensidx);
+ ARMARX_TRACE;
if (observerPublishSensorValues && publishToObserver)
{
for (const auto& pair : variants)
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index 26a7dd83ddcfd736b85cc4466686c9ea93c02809..1327e8e353890c42fb2200c253e42ff03325be35 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -445,11 +445,13 @@ namespace armarx
messages {other.messages, minimize},
buffer(other.buffer.size(), 0)
{
+ ARMARX_TRACE;
void* place = buffer.data();
std::size_t space = buffer.size();
auto getAlignedPlace = [&space, &place, this](std::size_t bytes, std::size_t alignment)
{
+ ARMARX_TRACE;
ARMARX_CHECK_EXPRESSION(std::align(alignment, bytes, place, space));
ARMARX_CHECK_LESS(bytes, space);
const auto resultPlace = place;
@@ -463,6 +465,7 @@ namespace armarx
sensors.reserve(other.sensors.size());
for (const SensorValueBase* sv : other.sensors)
{
+ ARMARX_TRACE;
sensors.emplace_back(sv->_placementCopyConstruct(getAlignedPlace(sv->_sizeof(), sv->_alignof())));
}
@@ -470,6 +473,7 @@ namespace armarx
control.reserve(other.control.size());
for (const auto& cdctargs : other.control)
{
+ ARMARX_TRACE;
control.emplace_back();
auto& ctargs = control.back();
ctargs.reserve(cdctargs.size());
diff --git a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp
index 85c6403e5e4452848c31ffbc3f59aa39c3fe46ee..41d39e44a9548099bc85ef8a22347796ba740717 100644
--- a/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp
+++ b/source/RobotAPI/libraries/aron/codegeneration/codegenerator/codewriter/cpp/Writer.cpp
@@ -277,6 +277,71 @@ namespace armarx::aron::codegenerator::cpp
CppMethodPtr enumAssignment3 = generator.toIntAssignmentMethod();
c->addMethod(enumAssignment3);
+ // Writermethods
+ for (codegenerator::WriterInfo info : dataWriters)
+ {
+ if (info.returnType == OWN_TYPE_NAME)
+ {
+ info.returnType = generator.getFullClassCppTypename();
+ }
+
+ if (!info.include.empty())
+ {
+ c->addInclude(info.include);
+ }
+ CppMethodPtr convert = generator.toSpecializedDataWriterMethod(info);
+ c->addMethod(convert);
+ }
+
+ // Add methods to set the member variables
+ // also resolve the original class name if the return type is set to special
+ for (codegenerator::StaticReaderInfo info : staticDataReaders)
+ {
+ if (info.returnType == OWN_TYPE_NAME)
+ {
+ info.returnType = generator.getFullClassCppTypename();
+ }
+ if (info.argumentType == OWN_TYPE_NAME)
+ {
+ info.argumentType = generator.getFullClassCppTypename();
+ }
+
+ CppMethodPtr convert = generator.toSpecializedStaticDataReaderMethod(info);
+ c->addMethod(convert);
+ }
+
+ // Add methods to set the member variables
+ for (codegenerator::ReaderInfo info : dataReaders)
+ {
+ if (info.argumentType == OWN_TYPE_NAME)
+ {
+ info.argumentType = generator.getFullClassCppTypename();
+ }
+
+ if (!info.include.empty())
+ {
+ c->addInclude(info.include);
+ }
+ CppMethodPtr convert = generator.toSpecializedDataReaderMethod(info);
+ c->addMethod(convert);
+ }
+
+ // Typewritermethods
+ for (codegenerator::WriterInfo info : initialTypeWriters)
+ {
+ if (info.returnType == OWN_TYPE_NAME)
+ {
+ info.returnType = generator.getFullClassCppTypename();
+ }
+
+ if (!info.include.empty())
+ {
+ c->addInclude(info.include);
+ }
+ CppMethodPtr convert = generator.toSpecializedTypeWriterMethod(info);
+ c->addMethod(convert);
+ }
+
typeClasses.push_back(c);
}
}