diff --git a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
index 8b69a5d565d944c415be16a0815f26b9d5c1af07..c4e6a36af757d2d020eb6ebb0fdb1a8a2310f5f2 100644
--- a/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/ControlTargets/ControlTargetBase.h
@@ -80,7 +80,15 @@ namespace armarx
virtual std::size_t getNumberOfDataFields() const = 0;
virtual std::vector<std::string> getDataFieldNames() const = 0;
- virtual std::string getDataFieldAsString(std::size_t i) const = 0;
+ virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Double& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, std::string& out) const = 0;
+ virtual const std::type_info& getDataFieldType(std::size_t i) const = 0;
//management functions
template<class T, class = typename std::enable_if<std::is_base_of<ControlTargetBase, T>::value>::type>
@@ -118,9 +126,41 @@ namespace armarx
{ \
return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
} \
- std::string getDataFieldAsString(std::size_t i) const override \
+ void getDataFieldAs (std::size_t i, bool& out) const override \
{ \
- return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAsString(this, i); \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Byte& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Short& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Int& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Long& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Float& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Double& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, std::string& out) const override \
+ { \
+ ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ const std::type_info& getDataFieldType(std::size_t i) const override \
+ { \
+ return ControlTargetInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
} \
std::vector<std::string> getDataFieldNames() const override \
{ \
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
index 1f58f79ae6bb6be402f4b22b45bc5e9ebb06ae7a..0994896f9a3510a8c4ce0241d981d0dad2bf03cc 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.cpp
@@ -20,6 +20,12 @@
* GNU General Public License
*/
+#include <regex>
+
+#include <boost/iterator/transform_iterator.hpp>
+
+#include <ArmarXCore/util/CPPUtility/trace.h>
+#include <ArmarXCore/util/CPPUtility/Iterator.h>
#include <ArmarXCore/core/util/FileSystemPathBuilder.h>
#include <ArmarXCore/core/ArmarXManager.h>
@@ -36,6 +42,7 @@ namespace armarx::RobotUnitModule
{
void Logging::addMarkerToRtLog(const RemoteReferenceCounterBasePtr& token, const std::string& marker, const Ice::Current&)
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
if (!rtLoggingEntries.count(token->getId()))
@@ -47,6 +54,7 @@ namespace armarx::RobotUnitModule
RemoteReferenceCounterBasePtr Logging::startRtLogging(const std::string& formatString, const Ice::StringSeq& loggingNames, const Ice::Current&)
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
StringStringDictionary alias;
for (const auto& name : loggingNames)
@@ -58,11 +66,12 @@ namespace armarx::RobotUnitModule
void Logging::stopRtLogging(const RemoteReferenceCounterBasePtr& token, const Ice::Current&)
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
if (!rtLoggingEntries.count(token->getId()))
{
- throw InvalidArgumentException {"addMarkerToRtLog called for a nonexistent log"};
+ throw InvalidArgumentException {"stopRtLogging called for a nonexistent log"};
}
ARMARX_DEBUG_S << "RobotUnit: stop RtLogging for file " << rtLoggingEntries.at(token->getId())->filename;
rtLoggingEntries.at(token->getId())->stopLogging = true;
@@ -70,24 +79,39 @@ namespace armarx::RobotUnitModule
Ice::StringSeq Logging::getLoggingNames(const Ice::Current&) const
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
Ice::StringSeq result;
- for (const auto& strs : sensorDeviceValueLoggingNames)
+ const auto getName = [](const auto & fieldData)
{
- result.insert(result.end(), strs.begin(), strs.end());
+ return fieldData.name;
+ };
+ for (const auto& data : sensorDeviceValueMetaData)
+ {
+ result.insert(
+ result.end(),
+ boost::make_transform_iterator(data.fields.begin(), getName),
+ boost::make_transform_iterator(data.fields.end(), getName));
}
- for (const auto& strvecss : controlDeviceValueLoggingNames)
+ for (const auto& datas : controlDeviceValueMetaData)
{
- for (const auto& strs : strvecss)
+ for (const auto& data : datas)
{
- result.insert(result.end(), strs.begin(), strs.end());
+ result.insert(
+ result.end(),
+ boost::make_transform_iterator(data.fields.begin(), getName),
+ boost::make_transform_iterator(data.fields.end(), getName));
}
}
return result;
}
- RemoteReferenceCounterBasePtr Logging::startRtLoggingWithAliasNames(const std::string& formatString, const StringStringDictionary& aliasNames, const Ice::Current&)
+ RemoteReferenceCounterBasePtr Logging::startRtLoggingWithAliasNames(
+ const std::string& formatString,
+ const StringStringDictionary& aliasNames,
+ const Ice::Current&)
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
FileSystemPathBuilder pb {formatString};
std::lock_guard<std::mutex> guard {rtLoggingMutex};
@@ -106,22 +130,19 @@ namespace armarx::RobotUnitModule
rtLoggingEntries.erase(pb.getPath());
throw LogicError {"RtLogging could not open filestream for '" + pb.getPath() + "'"};
}
+ ARMARX_INFO << "Start logging to " << e.filename
+ << ". Names (pattern, replacement name): " << aliasNames;
std::stringstream header;
- header << "marker;iteration;timestamp";
+ header << "marker;iteration;timestamp;TimeSinceLastIteration";
auto logDev = [&](const std::string & dev)
{
- for (const auto& pair : aliasNames)
+ ARMARX_TRACE_LITE;
+ for (const auto& [key, value] : aliasNames)
{
- std::string name = pair.first;
- if (boost::starts_with(dev + "$", name))
+ if (MatchName(key, dev))
{
- if ((name.size() > 0 && name.back() == '$'))
- {
- name.resize(name.size() - 1);
- }
- const std::string& alias = pair.second.empty() ? name : pair.second;
- header << ";" << alias << dev.substr(name.size());
+ header << ";" << (value.empty() ? dev : value);
return true;
}
}
@@ -130,40 +151,42 @@ namespace armarx::RobotUnitModule
//get logged sensor device values
{
- e.loggedSensorDeviceValues.reserve(sensorDeviceValueLoggingNames.size());
- for (const auto& svfields : sensorDeviceValueLoggingNames)
+ ARMARX_TRACE;
+ e.loggedSensorDeviceValues.reserve(sensorDeviceValueMetaData.size());
+ for (const auto& valData : sensorDeviceValueMetaData)
{
e.loggedSensorDeviceValues.emplace_back();
auto& svfieldsFlags = e.loggedSensorDeviceValues.back(); //vv
- svfieldsFlags.reserve(svfields.size());
- for (const auto& field : svfields)
+ svfieldsFlags.reserve(valData.fields.size());
+ for (const auto& field : valData.fields)
{
- svfieldsFlags.emplace_back(logDev(field));
+ svfieldsFlags.emplace_back(logDev(field.name));
}
}
}
//get logged control device values
{
- e.loggedControlDeviceValues.reserve(controlDeviceValueLoggingNames.size());
- for (const auto& sjctrls : controlDeviceValueLoggingNames)
+ ARMARX_TRACE;
+ e.loggedControlDeviceValues.reserve(controlDeviceValueMetaData.size());
+ for (const auto& datas : controlDeviceValueMetaData)
{
e.loggedControlDeviceValues.emplace_back();
- auto& sjctrlsFlags = e.loggedControlDeviceValues.back(); //vv
- sjctrlsFlags.reserve(sjctrls.size());
- for (const auto& ctargfields : sjctrls)
+ auto& deviceCtrlFlags = e.loggedControlDeviceValues.back(); //vv
+ deviceCtrlFlags.reserve(datas.size());
+ for (const auto& valData : datas)
{
+ deviceCtrlFlags.emplace_back();
+ auto& ctrlFieldFlags = deviceCtrlFlags.back(); //v
+ ctrlFieldFlags.reserve(valData.fields.size());
- sjctrlsFlags.emplace_back();
- auto& ctargfieldsFlags = sjctrlsFlags.back(); //v
- ctargfieldsFlags.reserve(ctargfields.size());
-
- for (const auto& field : ctargfields)
+ for (const auto& field : valData.fields)
{
- ctargfieldsFlags.emplace_back(logDev(field));
+ ctrlFieldFlags.emplace_back(logDev(field.name));
}
}
}
}
+ ARMARX_TRACE;
//write header
e.stream << header.str() << std::flush; // newline is written at the beginning of each log line
@@ -182,6 +205,7 @@ namespace armarx::RobotUnitModule
void Logging::writeRecentIterationsToFile(const std::string& formatString, const Ice::Current&) const
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
FileSystemPathBuilder pb {formatString};
@@ -200,20 +224,20 @@ namespace armarx::RobotUnitModule
//write csv header
{
outCSV << "iteration;timestamp";
- for (const auto& vs : sensorDeviceValueLoggingNames)
+ for (const auto& vs : sensorDeviceValueMetaData)
{
- for (const auto& s : vs)
+ for (const auto& f : vs.fields)
{
- outCSV << ";" << s;
+ outCSV << ";" << f.name;
}
}
- for (const auto& vvs : controlDeviceValueLoggingNames)
+ for (const auto& vvs : controlDeviceValueMetaData)
{
for (const auto& vs : vvs)
{
- for (const auto& s : vs)
+ for (const auto& f : vs.fields)
{
- outCSV << ";" << s;
+ outCSV << ";" << f.name;
}
}
}
@@ -231,7 +255,9 @@ namespace armarx::RobotUnitModule
{
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- outCSV << ";" << val->getDataFieldAsString(idxField);
+ std::string s;
+ val->getDataFieldAs(idxField, s);
+ outCSV << ";" << s;
}
}
}
@@ -243,7 +269,9 @@ namespace armarx::RobotUnitModule
{
for (std::size_t idxField = 0; idxField < val->getNumberOfDataFields(); ++ idxField)
{
- outCSV << ";" << val->getDataFieldAsString(idxField);
+ std::string s;
+ val->getDataFieldAs(idxField, s);
+ outCSV << ";" << s;
}
}
}
@@ -275,8 +303,134 @@ namespace armarx::RobotUnitModule
}
}
+ RobotUnitDataStreaming::DataStreamingDescription Logging::startDataStreaming(
+ const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const RobotUnitDataStreaming::Config& config,
+ const Ice::Current&)
+ {
+ ARMARX_TRACE;
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ if (!receiver)
+ {
+ throw InvalidArgumentException {"Receiver proxy is NULL!"};
+ }
+ std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ if (rtDataStreamingEntry.count(receiver))
+ {
+ throw InvalidArgumentException {"There already is a logger for the given receiver"};
+ }
+
+ RobotUnitDataStreaming::DataStreamingDescription result;
+ DataStreamingEntry& streamingEntry = rtDataStreamingEntry[receiver];
+
+ ARMARX_INFO << "start data streaming to " << receiver->ice_getIdentity().name
+ << ". Values: " << config.loggingNames;
+ auto devMatchesAnyKey = [&](const std::string & dev)
+ {
+ for (const auto& key : config.loggingNames)
+ {
+ if (MatchName(key, dev))
+ {
+ return true;
+ }
+ }
+ return false;
+ };
+
+ const auto handleVal = [&](
+ const ValueMetaData & valData,
+ DataStreamingEntry & streamingEntry,
+ RobotUnitDataStreaming::DataStreamingDescription & descr
+ ) -> std::vector<DataStreamingEntry::OutVal>
+ {
+ ARMARX_TRACE_LITE;
+ std::vector<DataStreamingEntry::OutVal> result;
+ result.resize(valData.fields.size());
+ for (std::size_t i = 0; i < valData.fields.size(); ++i)
+ {
+ if (!devMatchesAnyKey(valData.fields.at(i).name))
+ {
+ continue; //do not add to result and skipp during processing
+ }
+ auto& descrEntr = descr.entries[valData.fields.at(i).name];
+ //formatter failes here!
+ //*INDENT-OFF*
+ #define make_case(Type, TName) \
+ (typeid(Type) == *valData.fields.at(i).type) \
+ { \
+ descrEntr.index = streamingEntry.num##TName##s; \
+ descrEntr.type = RobotUnitDataStreaming::NodeType##TName; \
+ result.at(i).idx = streamingEntry.num##TName##s; \
+ result.at(i).value = DataStreamingEntry::ValueT::TName; \
+ ++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
+ {
+ 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;
+ };
+
+ //get logged sensor device values
+ {
+ ARMARX_TRACE;
+ streamingEntry.sensDevs.reserve(sensorDeviceValueMetaData.size());
+ for (const auto& valData : sensorDeviceValueMetaData)
+ {
+ streamingEntry.sensDevs.emplace_back(
+ handleVal(valData, streamingEntry, result));
+ }
+ }
+ //get logged control device values
+ {
+ ARMARX_TRACE;
+ streamingEntry.ctrlDevs.reserve(controlDeviceValueMetaData.size());
+ for (const auto& devData : controlDeviceValueMetaData)
+ {
+ streamingEntry.ctrlDevs.emplace_back();
+ auto& ctrlDevEntrs = streamingEntry.ctrlDevs.back();
+ ctrlDevEntrs.reserve(devData.size());
+ for (const auto& valData : devData)
+ {
+ ctrlDevEntrs.emplace_back(
+ handleVal(valData, streamingEntry, result));
+ }
+ }
+ }
+
+ return result;
+ }
+
+ void Logging::stopDataStreaming(const RobotUnitDataStreaming::ReceiverPrx& receiver, const Ice::Current&)
+ {
+ ARMARX_TRACE;
+ throwIfInControlThread(BOOST_CURRENT_FUNCTION);
+ std::lock_guard<std::mutex> guard {rtLoggingMutex};
+ if (!rtDataStreamingEntry.count(receiver))
+ {
+ throw InvalidArgumentException {"stopDataStreaming called for a nonexistent log"};
+ }
+ ARMARX_INFO_S << "RobotUnit: request to stop DataStreaming for " << receiver->ice_id();
+ rtDataStreamingEntry.at(receiver).stopStreaming = true;
+ }
+
void Logging::_preFinishRunning()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
defaultLogHandle = nullptr;
if (rtLoggingTask)
@@ -294,6 +448,7 @@ namespace armarx::RobotUnitModule
void Logging::_preFinishControlThreadInitialization()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
controlThreadId = LogSender::getThreadId();
ControlThreadOutputBuffer::RtLoggingInstance = &(_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer());
@@ -304,6 +459,7 @@ namespace armarx::RobotUnitModule
void Logging::doLogging()
{
+ ARMARX_TRACE;
throwIfInControlThread(BOOST_CURRENT_FUNCTION);
std::lock_guard<std::mutex> guard {rtLoggingMutex};
const auto now = IceUtil::Time::now();
@@ -318,117 +474,203 @@ namespace armarx::RobotUnitModule
}
//log all
{
+ ARMARX_TRACE;
+ if (!rtLoggingEntries.empty() || !rtDataStreamingEntry.empty())
+ {
+ ARMARX_INFO << deactivateSpam()
+ << "Number of logs " << rtLoggingEntries.size() << '\n'
+ << "Number of streams " << rtDataStreamingEntry.size();
+ }
_module<ControlThreadDataBuffer>().getControlThreadOutputBuffer().foreachNewLoggingEntry(
- [&](const ControlThreadOutputBuffer::Entry & data)
+ [&](const auto & data, auto i, auto num)
+ {
+ doLogging(now, data, i, num);
+ }
+ );
+ }
+ ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
+ //flush all files
+ {
+ for (auto& pair : rtLoggingEntries)
+ {
+ pair.second->stream << std::flush;
+ }
+ }
+
+ //remove entries
+ {
+ ARMARX_TRACE;
+ std::vector<std::string> toRemove;
+ toRemove.reserve(rtLoggingEntries.size());
+ for (auto& [key, value] : rtLoggingEntries)
{
- //base (marker;iteration;timestamp)
+ if (value->stopLogging)
{
- for (auto& pair : rtLoggingEntries)
- {
- CSVLoggingEntry& e = *pair.second;
- e.stream << "\n"
- << e.marker << ";"
- << data.iteration << ";"
- << data.sensorValuesTimestamp.toMicroSeconds();
- e.marker.clear();
- }
+ //can't remove the current elemet
+ //(this would invalidate the current iterator)
+ toRemove.emplace_back(key);
}
- //sens
+ }
+ for (const auto& rem : toRemove)
+ {
+ rtLoggingEntries.erase(rem);
+ }
+ }
+ //deal with data streaming
+ {
+ ARMARX_TRACE;
+ std::vector<RobotUnitDataStreaming::ReceiverPrx> toRemove;
+ toRemove.reserve(rtDataStreamingEntry.size());
+ for (auto& [prx, data] : rtDataStreamingEntry)
+ {
+ if (data.stopStreaming)
+ {
+ toRemove.emplace_back(prx);
+ }
+ else
{
- //sensors
- for (std::size_t idxDev = 0; idxDev < data.sensors.size(); ++ idxDev)
+ data.send(prx);
+ }
+ }
+ for (const auto& prx : toRemove)
+ {
+ rtDataStreamingEntry.erase(prx);
+ }
+ }
+ }
+
+ void Logging::doLogging(const IceUtil::Time& now, const
+ ControlThreadOutputBuffer::Entry& data,
+ std::size_t i, std::size_t num)
+ {
+ ARMARX_TRACE;
+ //header
+ {
+ ARMARX_TRACE;
+ //base (marker;iteration;timestamp)
+ for (auto& [_, e] : rtLoggingEntries)
+ {
+ e->stream << "\n"
+ << e->marker << ";"
+ << data.iteration << ";"
+ << data.sensorValuesTimestamp.toMicroSeconds() << ";"
+ << data.timeSinceLastIteration.toMicroSeconds();
+ e->marker.clear();
+ }
+ //streaming
+ for (auto& [_, e] : rtDataStreamingEntry)
+ {
+ e.processHeader(data);
+ }
+ }
+ //process devices
+ {
+ //sens
+ {
+ ARMARX_TRACE;
+ //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)
{
- 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)
+ std::string str;
+ val->getDataFieldAs(idxField, str);
+ for (auto& [_, entry] : rtLoggingEntries)
{
- const auto str = val->getDataFieldAsString(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
+ if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
{
- if (entry->loggedSensorDeviceValues.at(idxDev).at(idxField))
- {
- entry->stream << ";" << str;
- }
+ entry->stream << ";" << str;
}
}
+ for (auto& [_, data] : rtDataStreamingEntry)
+ {
+ data.processSens(*val, idxDev, idxField);
+ }
}
}
- //ctrl
+ }
+ //ctrl
+ {
+ ARMARX_TRACE;
+ //joint controllers
+ for (std::size_t idxDev = 0; idxDev < data.control.size(); ++ idxDev)
{
- //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)
{
- const auto& vals = data.control.at(idxDev);
- //control value (e.g. v_platform)
- for (std::size_t idxVal = 0; idxVal < vals.size(); ++idxVal)
+ 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 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)
+ std::string str;
+ val->getDataFieldAs(idxField, str);
+ for (auto& [_, entry] : rtLoggingEntries)
{
- const auto str = val->getDataFieldAsString(idxField);
- for (auto& [_, entry] : rtLoggingEntries)
+ if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
{
- if (entry->loggedControlDeviceValues.at(idxDev).at(idxVal).at(idxField))
- {
- entry->stream << ";" << str;
- }
+ entry->stream << ";" << str;
}
}
+ for (auto& [_, data] : rtDataStreamingEntry)
+ {
+ data.processCtrl(*val, idxDev, idxVal, idxField);
+ }
}
}
}
- //store data to backlog
+ }
+ }
+ //finish processing
+ {
+ //store data to backlog
+ {
+ ARMARX_TRACE;
+ if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
{
- if (data.writeTimestamp + rtLoggingBacklogRetentionTime >= now)
- {
- backlog.emplace_back(data, true); //true for minimal copy
- }
+ backlog.emplace_back(data, true); //true for minimal copy
}
- //print + reset messages
+ }
+ //print + reset messages
+ {
+ ARMARX_TRACE;
+ for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
{
- for (const ::armarx::detail::RtMessageLogEntryBase* ptr : data.messages.getEntries())
+ if (!ptr)
{
- if (!ptr)
- {
- break;
- }
- ptr->print(controlThreadId);
+ break;
}
+ ptr->print(controlThreadId);
}
- });
- }
- ARMARX_DEBUG_S << ::deactivateSpam() << "the last " << backlog.size() << " iterations are stored";
- //flush all files
- {
- for (auto& pair : rtLoggingEntries)
- {
- pair.second->stream << std::flush;
}
}
+ }
- //remove entries
+ bool Logging::MatchName(const std::string& pattern, const std::string& name)
+ {
+ ARMARX_TRACE;
+ if (pattern.empty())
{
- std::vector<std::string> toRemove;
- toRemove.reserve(rtLoggingEntries.size());
- for (auto& pair : rtLoggingEntries)
- {
- if (pair.second->stopLogging)
- {
- //can't remove the current elemet
- //(this would invalidate the current iterator)
- toRemove.emplace_back(pair.first);
- }
- }
- for (const auto& rem : toRemove)
- {
- rtLoggingEntries.erase(rem);
- }
+ 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);
rtLoggingTimestep = getProperty<std::size_t>("RTLogging_PeriodMs");
ARMARX_CHECK_LESS(0, rtLoggingTimestep) << "The property RTLoggingPeriodMs must not be 0";
@@ -448,9 +690,11 @@ namespace armarx::RobotUnitModule
void Logging::_postFinishDeviceInitialization()
{
+ 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 = rtLoggingTimestep * 1000;
std::size_t nBuffers = (logThreadPeriodUs / ctrlThreadPeriodUs + 1) * 100;
@@ -463,51 +707,54 @@ namespace armarx::RobotUnitModule
<< nBuffers << "buffers "
<< "(buffersize = " << bufferSize << " bytes)";
}
- //init logging names
+ //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());
+ for (const auto& [fieldIdx, fieldName] : MakeIndexedContainer(names))
+ {
+ data.fields.at(fieldIdx).name = namePre + '.' + fieldName;
+ data.fields.at(fieldIdx).type = &(val->getDataFieldType(fieldIdx));
+ }
+ return data;
+ };
+
//sensorDevices
- controlDeviceValueLoggingNames.reserve(_module<Devices>().getControlDevices().size());
+ controlDeviceValueMetaData.reserve(_module<Devices>().getControlDevices().size());
for (const auto& cd : _module<Devices>().getControlDevices().values())
{
- controlDeviceValueLoggingNames.emplace_back();
- auto& namesForDev = controlDeviceValueLoggingNames.back();
- namesForDev.reserve(cd->getJointControllers().size());
+ ARMARX_TRACE;
+ controlDeviceValueMetaData.emplace_back();
+ auto& dataForDev = controlDeviceValueMetaData.back();
+ dataForDev.reserve(cd->getJointControllers().size());
for (auto jointC : cd->getJointControllers())
{
- const auto names = jointC->getControlTarget()->getDataFieldNames();
- namesForDev.emplace_back();
- auto& fullNames = namesForDev.back();
- fullNames.reserve(names.size());
- for (const auto& name : names)
- {
- fullNames.emplace_back(
- "ctrl." +
- cd->getDeviceName() + "." +
- jointC->getControlMode() + "." +
- name);
- }
+ dataForDev.emplace_back(
+ makeValueMetaData(jointC->getControlTarget(),
+ "ctrl." +
+ cd->getDeviceName() + "." +
+ jointC->getControlMode()));
}
}
//sensorDevices
- sensorDeviceValueLoggingNames.reserve(_module<Devices>().getSensorDevices().size());
+ sensorDeviceValueMetaData.reserve(_module<Devices>().getSensorDevices().size());
for (const auto& sd : _module<Devices>().getSensorDevices().values())
{
- const auto names = sd->getSensorValue()->getDataFieldNames();
- sensorDeviceValueLoggingNames.emplace_back();
- auto& fullNames = sensorDeviceValueLoggingNames.back();
- fullNames.reserve(names.size());
- for (const auto& name : names)
- {
- fullNames.emplace_back(
- "sens." +
- sd->getDeviceName() + "." +
- name);
- }
+ ARMARX_TRACE;
+ sensorDeviceValueMetaData.emplace_back(
+ makeValueMetaData(sd->getSensorValue(),
+ "sens." +
+ sd->getDeviceName()));
}
}
//start logging thread is done in rtinit
//maybe add the default log
{
+ ARMARX_TRACE;
const auto loggingpath = getProperty<std::string>("RTLogging_DefaultLog").getValue();
if (!loggingpath.empty())
{
@@ -518,4 +765,117 @@ namespace armarx::RobotUnitModule
rtLoggingTask = new RTLoggingTaskT([&] {doLogging();}, rtLoggingTimestep, false, getName() + "_RTLoggingTask");
rtLoggingTask->setDelayWarningTolerance(rtLoggingTimestep * 10);
}
+
+ void Logging::DataStreamingEntry::processHeader(const ControlThreadOutputBuffer::Entry& e)
+ {
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ result.emplace_back();
+ auto& data = result.back();
+ data.iterationId = e.iteration;
+ data.timestampUSec = e.sensorValuesTimestamp.toMicroSeconds();
+ data.timesSinceLastIterationUSec = e.timeSinceLastIteration.toMicroSeconds();
+
+ 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);
+
+ }
+
+ void WriteTo(const Logging::DataStreamingEntry::OutVal& out,
+ const auto& val,
+ std::size_t fidx,
+ auto& data)
+ {
+ ARMARX_TRACE;
+ using enum_t = Logging::DataStreamingEntry::ValueT;
+ 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;
+ }
+ }
+
+ void Logging::DataStreamingEntry::processCtrl(
+ const ControlTargetBase& val,
+ std::size_t didx,
+ std::size_t vidx,
+ std::size_t fidx)
+ {
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = ctrlDevs.at(didx).at(vidx).at(fidx);
+ WriteTo(o, val, fidx, data);
+ }
+
+ void Logging::DataStreamingEntry::processSens(
+ const SensorValueBase& val,
+ std::size_t didx,
+ std::size_t fidx)
+ {
+ ARMARX_TRACE;
+ if (stopStreaming)
+ {
+ return;
+ }
+ auto& data = result.back();
+ const OutVal& o = sensDevs.at(didx).at(fidx);
+ WriteTo(o, val, fidx, data);
+ }
+
+ void Logging::DataStreamingEntry::send(const RobotUnitDataStreaming::ReceiverPrx& r)
+ {
+ ARMARX_TRACE;
+ try
+ {
+ r->update(result);
+ connectionFailures = 0;
+ result.clear();
+ }
+ catch (...)
+ {
+ ARMARX_TRACE;
+ ++connectionFailures;
+ if (connectionFailures > 10) ///TODO make property
+ {
+ stopStreaming = true;
+ ARMARX_WARNING_S << "DataStreaming Receiver was not reachable for "
+ << connectionFailures << " iterations! Removing receiver";
+ }
+ }
+ }
}
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
index 68cc02b917827edbb5442a7ef28d7e47d50d3776..2df9d88d2d8d497d12985fca320c95a8ea0e5576 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleLogging.h
@@ -150,12 +150,24 @@ namespace armarx::RobotUnitModule
*/
void writeRecentIterationsToFile(const std::string& formatString, const Ice::Current& = Ice::emptyCurrent) const override;
+ RobotUnitDataStreaming::DataStreamingDescription startDataStreaming(
+ const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const RobotUnitDataStreaming::Config& config,
+ const Ice::Current& = Ice::emptyCurrent) override;
+
+ void stopDataStreaming(
+ const RobotUnitDataStreaming::ReceiverPrx& receiver,
+ const Ice::Current& = Ice::emptyCurrent) override;
// //////////////////////////////////////////////////////////////////////////////////////// //
// //////////////////////////////////// implementation //////////////////////////////////// //
// //////////////////////////////////////////////////////////////////////////////////////// //
private:
/// @brief Executes the logging loop.
void doLogging();
+ void doLogging(const IceUtil::Time& now,
+ const ControlThreadOutputBuffer::Entry& data,
+ std::size_t i, std::size_t num);
+ static bool MatchName(const std::string& pattern, const std::string& name);
// //////////////////////////////////////////////////////////////////////////////////////// //
// ///////////////////////////////////////// Data ///////////////////////////////////////// //
@@ -180,6 +192,54 @@ namespace armarx::RobotUnitModule
std::string marker;
};
+ struct DataStreamingEntry
+ {
+ enum class ValueT
+ {
+ Bool,
+ Byte,
+ Short,
+ Int,
+ Long,
+ Float,
+ Double,
+ Skipped
+ };
+ struct OutVal
+ {
+ std::size_t idx;
+ ValueT value = ValueT::Skipped;
+ };
+
+ bool stopStreaming = false;
+
+ std::size_t numBools = 0;
+ std::size_t numBytes = 0;
+ std::size_t numShorts = 0;
+ std::size_t numInts = 0;
+ std::size_t numLongs = 0;
+ std::size_t numFloats = 0;
+ std::size_t numDoubles = 0;
+
+ bool onlyNewestFrame = false;
+ std::size_t connectionFailures = 0;
+
+ std::vector<std::vector<OutVal>> sensDevs;
+ std::vector<std::vector<std::vector<OutVal>>> ctrlDevs;
+
+ RobotUnitDataStreaming::TimeStepSeq result;
+ void processHeader(const ControlThreadOutputBuffer::Entry& e);
+ void processCtrl(const ControlTargetBase& val,
+ std::size_t didx,
+ std::size_t vidx,
+ std::size_t fidx);
+ void processSens(const SensorValueBase& val,
+ std::size_t didx,
+ std::size_t fidx);
+ void send(const RobotUnitDataStreaming::ReceiverPrx& r);
+ };
+ std::map<RobotUnitDataStreaming::ReceiverPrx, DataStreamingEntry> rtDataStreamingEntry;
+
/// @brief The thread executing the logging functions.
RTLoggingTaskT::pointer_type rtLoggingTask;
/// @brief All entries for logging.
@@ -192,10 +252,21 @@ namespace armarx::RobotUnitModule
Ice::Int controlThreadId {0};
/// @brief Mutex protecting the data structures of this class
mutable std::mutex rtLoggingMutex;
- /// @brief Data field names for all \ref ControlTargetBase "ControlTargets"
- std::vector<std::vector<std::vector<std::string>>> controlDeviceValueLoggingNames;
- /// @brief Data field names for all \ref SensorValueBase "SensorValues"
- std::vector<std::vector< std::string >> sensorDeviceValueLoggingNames;
+
+ struct ValueMetaData
+ {
+ struct FieldMetaData
+ {
+ std::string name;
+ const std::type_info* type;
+ };
+ std::vector<FieldMetaData> fields;
+ };
+
+ /// @brief Data field info for all \ref ControlTargetBase "ControlTargets" (dex x jctrl)
+ std::vector<std::vector<ValueMetaData>> controlDeviceValueMetaData;
+ /// @brief Data field info for all \ref SensorValueBase "SensorValues" (dev)
+ std::vector< ValueMetaData > sensorDeviceValueMetaData;
/// @brief The initial size of the Message entry buffer
std::size_t messageBufferSize {0};
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
index e378603fc90b365fe739a03fd60e76e34d913c9d..18d537b291f6c204036ec14e467a92e9660144df 100644
--- a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h
@@ -91,7 +91,15 @@ namespace armarx
virtual std::size_t getNumberOfDataFields() const = 0;
virtual std::vector<std::string> getDataFieldNames() const = 0;
- virtual std::string getDataFieldAsString(std::size_t i) const = 0;
+ virtual void getDataFieldAs(std::size_t i, bool& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Byte& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Short& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Int& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Long& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Float& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, Ice::Double& out) const = 0;
+ virtual void getDataFieldAs(std::size_t i, std::string& out) const = 0;
+ virtual const std::type_info& getDataFieldType(std::size_t i) const = 0;
//management functions
template<class T, class = typename std::enable_if<std::is_base_of<SensorValueBase, T>::value>::type>
@@ -130,9 +138,41 @@ namespace armarx
{ \
return SensorValueInfo<std::decay<decltype(*this)>::type>::GetNumberOfDataFields(); \
} \
- std::string getDataFieldAsString(std::size_t i) const override \
+ void getDataFieldAs (std::size_t i, bool& out) const override \
{ \
- return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAsString(this, i); \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Byte& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Short& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Int& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Long& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Float& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, Ice::Double& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ void getDataFieldAs (std::size_t i, std::string& out) const override \
+ { \
+ SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldAs (this, i, out); \
+ } \
+ const std::type_info& getDataFieldType(std::size_t i) const override \
+ { \
+ return SensorValueInfo<std::decay<decltype(*this)>::type>::GetDataFieldType(i); \
} \
std::vector<std::string> getDataFieldNames() const override \
{ \
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
index e753f9c4b60b13533dc2cfa20e6494a1d106ba1a..8f038519bc03b014aef5219bb939e1acf03d55af 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.cpp
@@ -71,7 +71,7 @@ namespace armarx
onePastLoggingReadPosition = writePosition.load();
}
- void ControlThreadOutputBuffer::foreachNewLoggingEntry(std::function<void (const ControlThreadOutputBuffer::Entry&)> consumer)
+ void ControlThreadOutputBuffer::foreachNewLoggingEntry(ConsumerFunctor consumer)
{
ARMARX_CHECK_EXPRESSION(isInitialized);
if (writePosition - onePastLoggingReadPosition >= numEntries)
@@ -87,10 +87,15 @@ namespace armarx
auto numNewEntries = writePosition - onePastLoggingReadPosition;
ARMARX_CHECK_EXPRESSION(numNewEntries < numEntries);
//consume all
- for (; onePastLoggingReadPosition < writePosition; ++onePastLoggingReadPosition)
+ const std::size_t num = writePosition - onePastLoggingReadPosition;
+ for (
+ std::size_t offset = 0;
+ onePastLoggingReadPosition < writePosition;
+ ++onePastLoggingReadPosition, ++offset
+ )
{
auto& entry = entries.at(toBounds(onePastLoggingReadPosition));
- consumer(entry);
+ consumer(entry, offset, num);
entry.messages.reset(messageBufferSize, messageBufferEntries, entry.iteration);
//update how many
diff --git a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
index 6412aaba778c5137020f43ae7e9efadf3e69c640..1709023ec2cc788dd4fa9664dbe6739746bad4ae 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/ControlThreadOutputBuffer.h
@@ -221,6 +221,7 @@ namespace armarx
{
using Entry = detail::ControlThreadOutputBufferEntry;
using RtMessageLogEntryBase = detail::RtMessageLogEntryBase;
+ using ConsumerFunctor = std::function<void (const Entry&, std::size_t, std::size_t)>;
std::size_t initialize(
std::size_t numEntries,
const KeyValueVector<std::string, ControlDevicePtr>& controlDevices,
@@ -245,7 +246,7 @@ namespace armarx
//logging read
void resetLoggingPosition() const;
- void foreachNewLoggingEntry(std::function<void(const Entry&)> consumer);
+ void foreachNewLoggingEntry(ConsumerFunctor consumer);
std::size_t getNumberOfBytes() const;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
index 65e8980af45c0a202787d0220e89bf7cf0313ea1..554b1ae3f27be04d0566d8306d8c7859f497f902 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfo.h
@@ -55,10 +55,39 @@ namespace armarx::introspection
/// @brief Get all entries for member variables
static const KeyValueVector<std::string, Entry>& GetEntries();
static std::size_t GetNumberOfDataFields();
- static std::string GetDataFieldAsString(const ClassType* ptr, std::size_t i);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, bool& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Byte& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Short& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Int& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Long& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Float& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Double& out);
+ static void GetDataFieldAs(const ClassType* ptr, std::size_t i, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
static std::vector<std::string> GetDataFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(const IceUtil::Time& timestamp, const CommonBaseT* ptr);
+ private:
+ template<class T>
+ static auto MakeConverter()
+ {
+ std::vector<std::function<void(const ClassType*, T&)>> functions;
+
+ for (std::size_t idxEntr = 0; idxEntr < GetEntries().size(); ++idxEntr)
+ {
+ for (std::size_t idxField = 0; idxField < GetEntries().at(idxEntr).getNumberOfFields(); ++idxField)
+ {
+ functions.emplace_back(
+ [idxEntr, idxField](const ClassType * classptr, T & out)
+ {
+ const Entry& e = GetEntries().at(idxEntr);
+ e.getDataFieldAs(idxField, classptr, out);
+ });
+ }
+ }
+ ARMARX_CHECK_EQUAL(functions.size(), GetNumberOfDataFields());
+ return functions;
+ }
private:
KeyValueVector<std::string, Entry> entries;
@@ -174,28 +203,81 @@ namespace armarx::introspection
}
template<class CommonBaseT, class ClassT>
- std::string ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAsString(const ClassType* ptr, std::size_t i)
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, bool& out)
+ {
+ static const auto convert = MakeConverter<bool>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Byte& out)
+ {
+ static const auto convert = MakeConverter<Ice::Byte>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Short& out)
+ {
+ static const auto convert = MakeConverter<Ice::Short>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Int& out)
+ {
+ static const auto convert = MakeConverter<Ice::Int>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Long& out)
+ {
+ static const auto convert = MakeConverter<Ice::Long>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Float& out)
+ {
+ static const auto convert = MakeConverter<Ice::Float>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, Ice::Double& out)
+ {
+ static const auto convert = MakeConverter<Ice::Double>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+ template<class CommonBaseT, class ClassT>
+ void ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldAs(const ClassType* ptr, std::size_t i, std::string& out)
+ {
+ static const auto convert = MakeConverter<std::string>();
+ ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
+ convert.at(i)(ptr, out);
+ }
+
+ template<class CommonBaseT, class ClassT>
+ const std::type_info& ClassMemberInfo<CommonBaseT, ClassT>::GetDataFieldType(std::size_t i)
{
- static const auto toString = []
+ static const auto convert = []
{
- std::vector<std::function<std::string(const ClassType*)>> functions;
+ std::vector<const std::type_info*> data;
for (std::size_t idxEntr = 0; idxEntr < GetEntries().size(); ++idxEntr)
{
+ const Entry& e = GetEntries().at(idxEntr);
for (std::size_t idxField = 0; idxField < GetEntries().at(idxEntr).getNumberOfFields(); ++idxField)
{
- functions.emplace_back(
- [idxEntr, idxField](const ClassType * classptr)
- {
- const Entry& e = GetEntries().at(idxEntr);
- return e.getFieldAsString(idxField, classptr);
- });
+ data.emplace_back(&e.getFieldTypeId(idxField));
}
}
- ARMARX_CHECK_EQUAL(functions.size(), GetNumberOfDataFields());
- return functions;
+ ARMARX_CHECK_EQUAL(data.size(), GetNumberOfDataFields());
+ return data;
}();
ARMARX_CHECK_LESS(i, GetNumberOfDataFields());
- return toString.at(i)(ptr);
+ return *convert.at(i);
}
}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
index 1f503de0562a64730dffb2c7f18749a7b7b63862..6188648d33e64d86dbac8a168446bbe5127e6339 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/ClassMemberInfoEntry.h
@@ -47,7 +47,7 @@ namespace armarx::introspection
memberName {memberName},
memberTypeName {GetTypeString<MemberType>()},
numberOfFields {DataFieldsInfo<MemberType>::GetNumberOfFields()},
- fieldToString {MakeFieldToStringFunctors<ClassType>(numberOfFields, ptr)},
+ fieldToType {MakeFieldToTypeFunctors<ClassType>(numberOfFields, ptr)},
toVariants_ {MakeToVariantsFunctor<ClassType>(ptr)}
{
ARMARX_CHECK_NOT_EQUAL(0, numberOfFields);
@@ -91,11 +91,58 @@ namespace armarx::introspection
ARMARX_CHECK_LESS(i, numberOfFields);
return fieldNames.at(i);
}
- std::string getFieldAsString(std::size_t i, const CommonBaseType* ptr) const
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, bool& out) const
{
ARMARX_CHECK_NOT_NULL(ptr);
ARMARX_CHECK_LESS(i, numberOfFields);
- return fieldToString.at(i)(ptr);
+ return fieldToType.at(i).toBool(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Byte& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toByte(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Short& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toShort(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Int& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toInt(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Long& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toLong(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Float& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toFloat(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, Ice::Double& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toDouble(ptr, out);
+ }
+ void getDataFieldAs(std::size_t i, const CommonBaseType* ptr, std::string& out) const
+ {
+ ARMARX_CHECK_NOT_NULL(ptr);
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return fieldToType.at(i).toString(ptr, out);
+ }
+ const std::type_info& getFieldTypeId(std::size_t i) const
+ {
+ ARMARX_CHECK_LESS(i, numberOfFields);
+ return *fieldToType.at(i).typeId;
}
//variants
@@ -110,25 +157,49 @@ namespace armarx::introspection
const ClassMemberInfoEntry* thisptr,
const IceUtil::Time&,
const CommonBaseType*) >;
- using FieldToStringFunctorType = std::function<std::string(const CommonBaseType*)>;
+ struct FieldToType
+ {
+ template<class T>
+ using FieldToFunctorType = std::function<void(const CommonBaseType*, T&)>;
+ FieldToFunctorType<bool> toBool;
+ FieldToFunctorType<Ice::Byte> toByte;
+ FieldToFunctorType<Ice::Short> toShort;
+ FieldToFunctorType<Ice::Int> toInt;
+ FieldToFunctorType<Ice::Long> toLong;
+ FieldToFunctorType<Ice::Float> toFloat;
+ FieldToFunctorType<Ice::Double> toDouble;
+ FieldToFunctorType<std::string> toString;
+ const std::type_info* typeId;
+ };
template<class ClassType, class MemberType, class MemberPtrClassType>
- static std::vector<FieldToStringFunctorType> MakeFieldToStringFunctors(
+ static std::vector<FieldToType> MakeFieldToTypeFunctors(
std::size_t numberOfFields,
MemberType MemberPtrClassType::* ptr)
{
- std::vector<std::function<std::string(const CommonBaseType*)>> result;
+ std::vector<FieldToType> result;
result.reserve(numberOfFields);
for (std::size_t i = 0; i < numberOfFields; ++i)
{
- result.emplace_back(
- [i, ptr](const CommonBaseType * ptrBase)
- {
- const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase);
- ARMARX_CHECK_NOT_NULL(cptr);
- return DataFieldsInfo<MemberType>::GetFieldAsString(i, cptr->*ptr);
- }
- );
+ result.emplace_back();
+ auto& fieldData = result.back();
+
+#define make_getter(Type) [i, ptr](const CommonBaseType * ptrBase, Type & out) \
+ { \
+ const ClassType* cptr = dynamic_cast<const ClassType*>(ptrBase); \
+ ARMARX_CHECK_NOT_NULL(cptr); \
+ DataFieldsInfo<MemberType>::GetDataFieldAs(i, cptr->*ptr, out); \
+ }
+ fieldData.toBool = make_getter(bool);
+ fieldData.toByte = make_getter(Ice::Byte);
+ fieldData.toShort = make_getter(Ice::Short);
+ fieldData.toInt = make_getter(Ice::Int);
+ fieldData.toLong = make_getter(Ice::Long);
+ fieldData.toFloat = make_getter(Ice::Float);
+ fieldData.toDouble = make_getter(Ice::Double);
+ fieldData.toString = make_getter(std::string);
+#undef make_getter
+ fieldData.typeId = &DataFieldsInfo<MemberType>::GetDataFieldType(i);
}
return result;
}
@@ -160,7 +231,7 @@ namespace armarx::introspection
const std::string memberTypeName;
//elementar subparts
const std::size_t numberOfFields;
- const std::vector<FieldToStringFunctorType> fieldToString;
+ const std::vector<FieldToType> fieldToType;
std::vector<std::string> fieldNames;
//variants
ToVariantsFunctorType toVariants_;
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
index cf3a4e9b0ba5db52287a006a879c0b841778bf29..ff2d751a934f26a400befa995220fd38b2594532 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.cpp
@@ -33,15 +33,31 @@
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterfaceStdOverloads.h>
-
+//Eigen::Vector3f
namespace armarx::introspection
{
- std::string DataFieldsInfo<Eigen::Vector3f, void>::GetFieldAsString(std::size_t i, const Eigen::Vector3f& field)
+ std::size_t DataFieldsInfo<Eigen::Vector3f, void>::GetNumberOfFields()
+ {
+ return 3;
+ }
+ void DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, std::string& out)
{
ARMARX_CHECK_LESS(i, 3);
- return to_string(field(i));
+ out = to_string(field(i));
+ }
+ void DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, float& out)
+ {
+ ARMARX_CHECK_LESS(i, 3);
+ out = field(i);
+ }
+ const std::type_info& DataFieldsInfo<Eigen::Vector3f, void>::GetDataFieldType(std::size_t i)
+ {
+ return typeid(float);
+ }
+ std::vector<std::string> DataFieldsInfo<Eigen::Vector3f, void>::GetFieldNames()
+ {
+ return {"x", "y", "z"};
}
-
std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector3f, void>::ToVariants(
const Eigen::Vector3f& value,
const std::string& name,
@@ -57,125 +73,178 @@ namespace armarx::introspection
return {{name, {new TimedVariant(Vector3{value}, timestamp)}}};
}
}
-
+//Eigen::Vector##Num##Type
namespace armarx::introspection
{
-#define make_DataFieldsInfo_for_eigen_vector(Type,Num) \
- std::string DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldAsString(std::size_t i, const Eigen::Vector##Num##Type& field) \
- { \
- ARMARX_CHECK_LESS(i, Num); \
- return to_string(field(i)); \
- } \
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
- const Eigen::Vector##Num##Type& value, \
- const std::string& name, \
- const IceUtil::Time& timestamp, \
- const std::string& frame, \
- const std::string& agent) \
- { \
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant"; \
- std::map<std::string, VariantBasePtr> result; \
- for(int i = 0; i < Num; ++i) \
- { \
- result.emplace(name + "." + to_string(i), VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
- } \
- return result; \
- } \
- std::vector<std::string> DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
- { \
- std::vector<std::string> result; \
- for(int i = 0; i < Num; ++i) \
- { \
- result.emplace_back(to_string(i)); \
- } \
- return result; \
+#define make_DataFieldsInfo_for_eigen_vector(Type,TypeName,Num) \
+ void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out) \
+ { \
+ ARMARX_CHECK_LESS(i, Num); \
+ out = to_string(field(i)); \
+ } \
+ void DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out) \
+ { \
+ ARMARX_CHECK_LESS(i, Num); \
+ out = field(i); \
+ } \
+ const std::type_info& DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetDataFieldType(std::size_t i) \
+ { \
+ return typeid(Ice::TypeName); \
+ } \
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Vector##Num##Type, void>::ToVariants( \
+ const Eigen::Vector##Num##Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame, \
+ const std::string& agent) \
+ { \
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version of TimestampVariant"; \
+ std::map<std::string, VariantBasePtr> result; \
+ for(int i = 0; i < Num; ++i) \
+ { \
+ result.emplace(name + "." + to_string(i), VariantBasePtr{new TimedVariant(value(i), timestamp)}); \
+ } \
+ return result; \
+ } \
+ std::vector<std::string> DataFieldsInfo<Eigen::Vector##Num##Type, void>::GetFieldNames() \
+ { \
+ std::vector<std::string> result; \
+ for(int i = 0; i < Num; ++i) \
+ { \
+ result.emplace_back(to_string(i)); \
+ } \
+ return result; \
}
- make_DataFieldsInfo_for_eigen_vector(f, 2)
- make_DataFieldsInfo_for_eigen_vector(f, 4)
- make_DataFieldsInfo_for_eigen_vector(f, 5)
- make_DataFieldsInfo_for_eigen_vector(f, 6)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 2)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 4)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
- make_DataFieldsInfo_for_eigen_vector(d, 2)
- make_DataFieldsInfo_for_eigen_vector(d, 3)
- make_DataFieldsInfo_for_eigen_vector(d, 4)
- make_DataFieldsInfo_for_eigen_vector(d, 5)
- make_DataFieldsInfo_for_eigen_vector(d, 6)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
- make_DataFieldsInfo_for_eigen_vector(i, 2)
- make_DataFieldsInfo_for_eigen_vector(i, 3)
- make_DataFieldsInfo_for_eigen_vector(i, 4)
- make_DataFieldsInfo_for_eigen_vector(i, 5)
- make_DataFieldsInfo_for_eigen_vector(i, 6)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
#undef make_DataFieldsInfo_for_eigen_vector
}
-
+//Eigen::Matrix4f
namespace armarx::introspection
{
- std::string DataFieldsInfo<JointStatus, void>::GetFieldAsString(
- std::size_t i, const JointStatus& field)
+ std::size_t DataFieldsInfo<Eigen::Matrix4f, void>::GetNumberOfFields()
+ {
+ return 16;
+ }
+ void DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, float& out)
+ {
+ ARMARX_CHECK_LESS(i, 16);
+ out = field(i / 4, i % 4);
+ }
+ void DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, std::string& out)
+ {
+ ARMARX_CHECK_LESS(i, 16);
+ out = to_string(field(i / 4, i % 4));
+ }
+ const std::type_info& DataFieldsInfo<Eigen::Matrix4f, void>::GetDataFieldType(std::size_t i)
+ {
+ return typeid(float);
+ }
+ std::vector<std::string> DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldNames()
+ {
+ return
+ {
+ "00", "01", "02", "03",
+ "10", "11", "12", "13",
+ "20", "21", "22", "23",
+ "30", "31", "32", "33"
+ };
+ }
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(
+ const Eigen::Matrix4f& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ if (!frame.empty())
+ {
+ return {{name, {new TimedVariant(FramedPose{value, frame, agent}, timestamp)}}};
+ }
+ ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
+ return {{name, {new TimedVariant(Pose{value}, timestamp)}}};
+ }
+}
+//Eigen::Quaternionf
+namespace armarx::introspection
+{
+ std::size_t DataFieldsInfo<Eigen::Quaternionf, void>::GetNumberOfFields()
+ {
+ return 4;
+ }
+ void DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, float& out)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
{
case 0:
- return to_string(field.error);
+ out = field.w();
+ break;
case 1:
- return to_string(field.operation);
+ out = field.x();
+ break;
case 2:
- return to_string(field.enabled);
+ out = field.y();
+ break;
case 3:
- return to_string(field.emergencyStop);
+ out = field.z();
+ break;
}
throw std::logic_error
{
__FILE__ " : " + to_string(__LINE__) +
" : Unreachable code reached"
};
- }
- std::map<std::string, VariantBasePtr> DataFieldsInfo<JointStatus, void>::ToVariants(
- const JointStatus& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
- {
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version for JointStatus";
- static const auto fnames = GetFieldNames();
- return
- {
- {name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
- {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
- {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
- {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}
- };
}
-}
-
-namespace armarx::introspection
-{
- std::string DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldAsString(
- std::size_t i,
- const Eigen::Quaternionf& field)
+ void DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out)
{
ARMARX_CHECK_LESS(i, 4);
switch (i)
{
case 0:
- return to_string(field.w());
+ out = to_string(field.w());
+ break;
case 1:
- return to_string(field.x());
+ out = to_string(field.x());
+ break;
case 2:
- return to_string(field.y());
+ out = to_string(field.y());
+ break;
case 3:
- return to_string(field.z());
+ out = to_string(field.z());
+ break;
}
throw std::logic_error
{
__FILE__ " : " + to_string(__LINE__) +
" : Unreachable code reached"
};
+
+ }
+
+ const std::type_info& DataFieldsInfo<Eigen::Quaternionf, void>::GetDataFieldType(std::size_t i)
+ {
+ return typeid(float);
+ }
+ std::vector<std::string> DataFieldsInfo<Eigen::Quaternionf, void>::GetFieldNames()
+ {
+ return {"qw", "qx", "qy", "qz"};
}
std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Quaternionf, void>::ToVariants(
@@ -193,33 +262,27 @@ namespace armarx::introspection
return {{name, {new TimedVariant(Quaternion{value}, timestamp)}}};
}
}
-
+//std::chrono::microseconds
namespace armarx::introspection
{
- std::string DataFieldsInfo<Eigen::Matrix4f, void>::GetFieldAsString(std::size_t i, const Eigen::Matrix4f& field)
+ std::size_t DataFieldsInfo<std::chrono::microseconds, void>::GetNumberOfFields()
{
- ARMARX_CHECK_LESS(i, 16);
- return to_string(field(i / 4, i % 4));
+ return 1;
}
-
- std::map<std::string, VariantBasePtr> DataFieldsInfo<Eigen::Matrix4f, void>::ToVariants(
- const Eigen::Matrix4f& value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame,
- const std::string& agent)
+ void DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out)
{
- if (!frame.empty())
- {
- return {{name, {new TimedVariant(FramedPose{value, frame, agent}, timestamp)}}};
- }
- ARMARX_CHECK_EXPRESSION(agent.empty()) << "No frame but an agent given";
- return {{name, {new TimedVariant(Pose{value}, timestamp)}}};
+ ARMARX_CHECK_EQUAL(i, 0);
+ out = field.count();
+ }
+ void DataFieldsInfo<std::chrono::microseconds, void>::GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out)
+ {
+ ARMARX_CHECK_EQUAL(i, 0);
+ out = to_string(field.count());
+ }
+ const std::type_info& GetDataFieldType(std::size_t i)
+ {
+ return typeid(long);
}
-}
-
-namespace armarx::introspection
-{
std::map<std::string, VariantBasePtr> DataFieldsInfo<std::chrono::microseconds, void>::ToVariants(
std::chrono::microseconds value,
const std::string& name,
@@ -231,6 +294,28 @@ namespace armarx::introspection
return {{name, {new TimedVariant(TimestampVariant{value.count()}, timestamp)}}};
}
+}
+//IceUtil::Time
+namespace armarx::introspection
+{
+ std::size_t DataFieldsInfo<IceUtil::Time, void>::GetNumberOfFields()
+ {
+ return 1;
+ }
+ void DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i, const IceUtil::Time& field, long& out)
+ {
+ ARMARX_CHECK_EQUAL(i, 0);
+ out = field.toMicroSeconds();
+ }
+ void DataFieldsInfo<IceUtil::Time, void>::GetDataFieldAs(std::size_t i, const IceUtil::Time& field, std::string& out)
+ {
+ ARMARX_CHECK_EQUAL(i, 0);
+ out = to_string(field.toMicroSeconds());
+ }
+ const std::type_info& DataFieldsInfo<IceUtil::Time, void>::GetDataFieldType(std::size_t i)
+ {
+ return typeid(long);
+ }
std::map<std::string, VariantBasePtr> DataFieldsInfo<IceUtil::Time, void>::ToVariants(
IceUtil::Time value,
const std::string& name,
@@ -242,3 +327,121 @@ namespace armarx::introspection
return {{name, {new TimedVariant(TimestampVariant{value.toMicroSeconds()}, timestamp)}}};
}
}
+//JointStatus
+namespace armarx::introspection
+{
+ std::size_t DataFieldsInfo<JointStatus, void>::GetNumberOfFields()
+ {
+ return 4;
+ }
+ void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
+ std::size_t i, const JointStatus& field, std::string& out)
+ {
+ ARMARX_CHECK_LESS(i, 4);
+ switch (i)
+ {
+ case 0:
+ out = to_string(field.error);
+ break;
+ case 1:
+ out = to_string(field.operation);
+ break;
+ case 2:
+ out = to_string(field.enabled);
+ break;
+ case 3:
+ out = to_string(field.emergencyStop);
+ break;
+ }
+ throw std::logic_error
+ {
+ __FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"
+ };
+ }
+ void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
+ std::size_t i, const JointStatus& field, Ice::Int& out)
+ {
+ ARMARX_CHECK_EXPRESSION(i == 0 || i == 1);
+ switch (i)
+ {
+ case 0:
+ out = field.enabled;
+ break;
+ case 1:
+ out = field.emergencyStop;
+ break;
+ }
+ throw std::logic_error
+ {
+ __FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"
+ };
+ }
+ void DataFieldsInfo<JointStatus, void>::GetDataFieldAs(
+ std::size_t i, const JointStatus& field, bool& out)
+ {
+ ARMARX_CHECK_EXPRESSION(i == 2 || i == 3);
+ switch (i)
+ {
+ case 2:
+ out = static_cast<Ice::Int>(field.error);
+ break;
+ case 3:
+ out = static_cast<Ice::Int>(field.operation);
+ break;
+ }
+ throw std::logic_error
+ {
+ __FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"
+ };
+ }
+ const std::type_info& DataFieldsInfo<JointStatus, void>::GetDataFieldType(std::size_t i)
+ {
+ ARMARX_CHECK_LESS(i, 4);
+ switch (i)
+ {
+ case 0:
+ return typeid(Ice::Int);
+ break;
+ case 1:
+ return typeid(Ice::Int);
+ break;
+ case 2:
+ return typeid(bool);
+ break;
+ case 3:
+ return typeid(bool);
+ break;
+ }
+ throw std::logic_error
+ {
+ __FILE__ " : " + to_string(__LINE__) +
+ " : Unreachable code reached"
+ };
+ }
+
+ std::vector<std::string> DataFieldsInfo<JointStatus, void>::GetFieldNames()
+ {
+ return {"error", "operation", "enabled", "emergencyStop"};
+ }
+
+ std::map<std::string, VariantBasePtr> DataFieldsInfo<JointStatus, void>::ToVariants(
+ const JointStatus& value,
+ const std::string& name,
+ const IceUtil::Time& timestamp,
+ const std::string& frame,
+ const std::string& agent)
+ {
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version for JointStatus";
+ static const auto fnames = GetFieldNames();
+ return
+ {
+ {name + fnames.at(0), {new TimedVariant{to_string(value.error), timestamp}}},
+ {name + fnames.at(1), {new TimedVariant{to_string(value.operation), timestamp}}},
+ {name + fnames.at(2), {new TimedVariant{value.enabled, timestamp}}},
+ {name + fnames.at(3), {new TimedVariant{value.emergencyStop, timestamp}}}
+ };
+ }
+}
diff --git a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
index c122c55b0361fbe82f7e7ce6bbffcb2fdbf9bc84..16d76a7b7bba97433af9a97e26c4fe66c387b051 100644
--- a/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
+++ b/source/RobotAPI/components/units/RobotUnit/util/introspection/DataFieldsInfo.h
@@ -35,7 +35,15 @@ namespace armarx::introspection
{
template<class T, class = void> struct DataFieldsInfo;
// static std::size_t GetNumberOfFields();
- // static std::string GetFieldAsString(std::size_t i, T field);
+ // static void GetDataFieldAs(std::size_t i, T field, bool& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Byte& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Short& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Int& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Long& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Float& out);
+ // static void GetDataFieldAs(std::size_t i, T field, Ice::Double& out);
+ // static void GetDataFieldAs(std::size_t i, T field, std::string& out);
+ // static const std::type_info& GetDataFieldType(std::size_t i);
// static std::vector<std::string> GetFieldNames();
// static std::map<std::string, VariantBasePtr> ToVariants(
// T value,
@@ -45,45 +53,106 @@ namespace armarx::introspection
// const std::string& agent = "")
template<class T>
- struct DataFieldsInfo<T, typename std::enable_if<std::is_fundamental<T>::value>::type>
+ struct DataFieldsInfoBase
{
- static std::size_t GetNumberOfFields()
+ static void GetDataFieldAs(std::size_t i, const T& field, bool& out)
{
- return 1;
+ throw std::logic_error {"This function should never be called"};
}
- static std::string GetFieldAsString(std::size_t i, T field)
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Byte& out)
{
- ARMARX_CHECK_EQUAL(i, 0);
- return to_string(field);
+ throw std::logic_error {"This function should never be called"};
}
- static std::vector<std::string> GetFieldNames()
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Short& out)
{
- throw std::logic_error {"Fundamental types have no field names"};
+ throw std::logic_error {"This function should never be called"};
}
- static std::map<std::string, VariantBasePtr> ToVariants(
- T value,
- const std::string& name,
- const IceUtil::Time& timestamp,
- const std::string& frame = "",
- const std::string& agent = "")
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Int& out)
{
- ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) << "There is no framed version for fundamental types";
- return {{name, {new TimedVariant(value, timestamp)}}};
+ throw std::logic_error {"This function should never be called"};
}
- };
-
- template<>
- struct DataFieldsInfo<Eigen::Vector3f, void>
- {
- static std::size_t GetNumberOfFields()
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Long& out)
+ {
+ throw std::logic_error {"This function should never be called"};
+ }
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Float& out)
+ {
+ throw std::logic_error {"This function should never be called"};
+ }
+ static void GetDataFieldAs(std::size_t i, const T& field, Ice::Double& out)
+ {
+ throw std::logic_error {"This function should never be called"};
+ }
+ static void GetDataFieldAs(std::size_t i, const T& field, std::string& out)
{
- return 3;
+ throw std::logic_error {"This function should never be called"};
}
- static std::string GetFieldAsString(std::size_t i, const Eigen::Vector3f& field);
static std::vector<std::string> GetFieldNames()
{
- return {"x", "y", "z"};
+ throw std::logic_error {"This function should never be called"};
}
+ };
+}
+//build_in_ice_types
+namespace armarx::introspection
+{
+#define make_def_for_build_in_ice_type(Type) \
+ template<> \
+ struct DataFieldsInfo<Type, void> : DataFieldsInfoBase<Type> \
+ { \
+ using DataFieldsInfoBase<Type>::GetDataFieldAs; \
+ static std::size_t GetNumberOfFields() \
+ { \
+ return 1; \
+ } \
+ static void GetDataFieldAs(std::size_t i, const Type& field, std::string& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = to_string(field); \
+ } \
+ static void GetDataFieldAs(std::size_t i, const Type& field, Type& out) \
+ { \
+ ARMARX_CHECK_EQUAL(i, 0); \
+ out = field; \
+ } \
+ static const std::type_info& GetDataFieldType(std::size_t i) \
+ { \
+ return typeid(Type); \
+ } \
+ static std::map<std::string, VariantBasePtr> ToVariants( \
+ const Type& value, \
+ const std::string& name, \
+ const IceUtil::Time& timestamp, \
+ const std::string& frame = "", \
+ const std::string& agent = "") \
+ { \
+ ARMARX_CHECK_EXPRESSION(frame.empty() && agent.empty()) \
+ << "There is no framed version for build in ice types"; \
+ return {{name, {new TimedVariant(value, timestamp)}}}; \
+ } \
+ }
+ make_def_for_build_in_ice_type(bool);
+ make_def_for_build_in_ice_type(Ice::Byte);
+ make_def_for_build_in_ice_type(Ice::Short);
+ make_def_for_build_in_ice_type(Ice::Int);
+ make_def_for_build_in_ice_type(Ice::Long);
+ make_def_for_build_in_ice_type(Ice::Float);
+ make_def_for_build_in_ice_type(Ice::Double);
+#undef make_def_for_build_in_ice_type
+
+}
+//Eigen::Vector3f
+namespace armarx::introspection
+{
+ template<>
+ struct DataFieldsInfo<Eigen::Vector3f, void> : DataFieldsInfoBase<Eigen::Vector3f>
+ {
+ using DataFieldsInfoBase<Eigen::Vector3f>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, float& out);
+ static void GetDataFieldAs(std::size_t i, const Eigen::Vector3f& field, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
+ static std::vector<std::string> GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const Eigen::Vector3f& value,
const std::string& name,
@@ -91,20 +160,24 @@ namespace armarx::introspection
const std::string& frame = "",
const std::string& agent = "");
};
-
-
-
-#define make_DataFieldsInfo_for_eigen_vector(Type,Num) \
- template<> \
- struct DataFieldsInfo<Eigen::Vector##Num##Type, void> \
- { \
- static std::size_t GetNumberOfFields() \
- { \
- return Num; \
- } \
- static std::string GetFieldAsString(std::size_t i, const Eigen::Vector##Num##Type& field); \
- static std::vector<std::string> GetFieldNames(); \
- static std::map<std::string, VariantBasePtr> ToVariants( \
+}
+//Eigen::Vector##Num##Type
+namespace armarx::introspection
+{
+#define make_DataFieldsInfo_for_eigen_vector(Type,TypeName,Num) \
+ template<> \
+ struct DataFieldsInfo<Eigen::Vector##Num##Type, void> : DataFieldsInfoBase<Eigen::Vector##Num##Type> \
+ { \
+ using DataFieldsInfoBase<Eigen::Vector##Num##Type>::GetDataFieldAs; \
+ static std::size_t GetNumberOfFields() \
+ { \
+ return Num; \
+ } \
+ static void GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, std::string& out); \
+ static void GetDataFieldAs(std::size_t i, const Eigen::Vector##Num##Type& field, Ice::TypeName& out); \
+ static const std::type_info& GetDataFieldType(std::size_t i); \
+ static std::vector<std::string> GetFieldNames(); \
+ static std::map<std::string, VariantBasePtr> ToVariants( \
const Eigen::Vector##Num##Type& value, \
const std::string& name, \
const IceUtil::Time& timestamp, \
@@ -112,42 +185,37 @@ namespace armarx::introspection
const std::string& agent = ""); \
};
- make_DataFieldsInfo_for_eigen_vector(f, 2)
- make_DataFieldsInfo_for_eigen_vector(f, 4)
- make_DataFieldsInfo_for_eigen_vector(f, 5)
- make_DataFieldsInfo_for_eigen_vector(f, 6)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 2)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 4)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 5)
+ make_DataFieldsInfo_for_eigen_vector(f, Float, 6)
- make_DataFieldsInfo_for_eigen_vector(d, 2)
- make_DataFieldsInfo_for_eigen_vector(d, 3)
- make_DataFieldsInfo_for_eigen_vector(d, 4)
- make_DataFieldsInfo_for_eigen_vector(d, 5)
- make_DataFieldsInfo_for_eigen_vector(d, 6)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 2)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 3)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 4)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 5)
+ make_DataFieldsInfo_for_eigen_vector(d, Double, 6)
- make_DataFieldsInfo_for_eigen_vector(i, 2)
- make_DataFieldsInfo_for_eigen_vector(i, 3)
- make_DataFieldsInfo_for_eigen_vector(i, 4)
- make_DataFieldsInfo_for_eigen_vector(i, 5)
- make_DataFieldsInfo_for_eigen_vector(i, 6)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 2)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 3)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 4)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 5)
+ make_DataFieldsInfo_for_eigen_vector(i, Int, 6)
#undef make_DataFieldsInfo_for_eigen_vector
+}
+//Eigen::Matrix4f
+namespace armarx::introspection
+{
template<>
- struct DataFieldsInfo<Eigen::Matrix4f, void>
+ struct DataFieldsInfo<Eigen::Matrix4f, void> : DataFieldsInfoBase<Eigen::Matrix4f>
{
- static std::size_t GetNumberOfFields()
- {
- return 16;
- }
- static std::string GetFieldAsString(std::size_t i, const Eigen::Matrix4f& field);
- static std::vector<std::string> GetFieldNames()
- {
- return
- {
- "00", "01", "02", "03",
- "10", "11", "12", "13",
- "20", "21", "22", "23",
- "30", "31", "32", "33"
- };
- }
+ using DataFieldsInfoBase<Eigen::Matrix4f>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, float& out);
+ static void GetDataFieldAs(std::size_t i, const Eigen::Matrix4f& field, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
+ static std::vector<std::string> GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const Eigen::Matrix4f& value,
const std::string& name,
@@ -155,19 +223,19 @@ namespace armarx::introspection
const std::string& frame = "",
const std::string& agent = "");
};
-
+}
+//Eigen::Quaternionf
+namespace armarx::introspection
+{
template<>
- struct DataFieldsInfo<Eigen::Quaternionf, void>
+ struct DataFieldsInfo<Eigen::Quaternionf, void> : DataFieldsInfoBase<Eigen::Quaternionf>
{
- static std::size_t GetNumberOfFields()
- {
- return 4;
- }
- static std::string GetFieldAsString(std::size_t i, const Eigen::Quaternionf& field);
- static std::vector<std::string> GetFieldNames()
- {
- return {"qw", "qx", "qy", "qz"};
- }
+ using DataFieldsInfoBase<Eigen::Quaternionf>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, float& out);
+ static void GetDataFieldAs(std::size_t i, const Eigen::Quaternionf& field, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
+ static std::vector<std::string> GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const Eigen::Quaternionf& value,
const std::string& name,
@@ -175,22 +243,18 @@ namespace armarx::introspection
const std::string& frame = "",
const std::string& agent = "");
};
-
+}
+//std::chrono::microseconds
+namespace armarx::introspection
+{
template<>
- struct DataFieldsInfo<std::chrono::microseconds, void>
+ struct DataFieldsInfo<std::chrono::microseconds, void> : DataFieldsInfoBase<std::chrono::microseconds>
{
- static std::size_t GetNumberOfFields()
- {
- return 1;
- }
- static std::string GetFieldAsString(std::size_t i, std::chrono::microseconds field)
- {
- return to_string(field.count());
- }
- static std::vector<std::string> GetFieldNames()
- {
- throw std::logic_error {"should never be called"};
- }
+ using DataFieldsInfoBase<std::chrono::microseconds>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, long& out);
+ static void GetDataFieldAs(std::size_t i, const std::chrono::microseconds& field, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
static std::map<std::string, VariantBasePtr> ToVariants(
std::chrono::microseconds value,
const std::string& name,
@@ -198,21 +262,18 @@ namespace armarx::introspection
const std::string& frame = "",
const std::string& agent = "");
};
+}
+//IceUtil::Time
+namespace armarx::introspection
+{
template<>
- struct DataFieldsInfo<IceUtil::Time, void>
+ struct DataFieldsInfo<IceUtil::Time, void> : DataFieldsInfoBase<IceUtil::Time>
{
- static std::size_t GetNumberOfFields()
- {
- return 1;
- }
- static std::string GetFieldAsString(std::size_t i, IceUtil::Time field)
- {
- return to_string(field.toMicroSeconds());
- }
- static std::vector<std::string> GetFieldNames()
- {
- throw std::logic_error {"should never be called"};
- }
+ using DataFieldsInfoBase<IceUtil::Time>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const IceUtil::Time& field, long& out);
+ static void GetDataFieldAs(std::size_t i, const IceUtil::Time& field, std::string& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
static std::map<std::string, VariantBasePtr> ToVariants(
IceUtil::Time value,
const std::string& name,
@@ -221,26 +282,23 @@ namespace armarx::introspection
const std::string& agent = "");
};
}
-
+//JointStatus
namespace armarx
{
struct JointStatus;
}
-
namespace armarx::introspection
{
template<>
- struct DataFieldsInfo<JointStatus, void>
+ struct DataFieldsInfo<JointStatus, void> : DataFieldsInfoBase<JointStatus>
{
- static std::size_t GetNumberOfFields()
- {
- return 4;
- }
- static std::string GetFieldAsString(std::size_t i, const JointStatus& field);
- static std::vector<std::string> GetFieldNames()
- {
- return {"error", "operation", "enabled", "emergencyStop"};
- }
+ using DataFieldsInfoBase<JointStatus>::GetDataFieldAs;
+ static std::size_t GetNumberOfFields();
+ static void GetDataFieldAs(std::size_t i, const JointStatus& field, std::string& out);
+ static void GetDataFieldAs(std::size_t i, const JointStatus& field, Ice::Int& out);
+ static void GetDataFieldAs(std::size_t i, const JointStatus& field, bool& out);
+ static const std::type_info& GetDataFieldType(std::size_t i);
+ static std::vector<std::string> GetFieldNames();
static std::map<std::string, VariantBasePtr> ToVariants(
const JointStatus& value,
@@ -250,4 +308,3 @@ namespace armarx::introspection
const std::string& agent);
};
}
-
diff --git a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
index 361cf690a3498a2a9ffcb95527ef27005c54fe86..c7871f0860fbe343a1704b2022f54a8ed6cd8674 100644
--- a/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/RobotUnitInterface.ice
@@ -46,6 +46,7 @@
#include <ArmarXCore/interface/core/UserException.ice>
#include <ArmarXGui/interface/WidgetDescription.ice>
+//NJointController
module armarx
{
interface NJointControllerInterface;
@@ -61,7 +62,7 @@ module armarx
};
- class NJointControllerConfig{};
+ class NJointControllerConfig {};
struct NJointControllerDescription
{
@@ -117,10 +118,10 @@ module armarx
dictionary<string, NJointControllerInterface*> StringNJointControllerPrxDictionary;
};
+//RobotUnit Utility types - ControlDevice
module armarx
{
dictionary<string, Ice::StringSeq> ControlDeviceNameToControlModesDictionary;
-
dictionary<string, string> ControlDeviceNameToNJointControllerNameDictionary;
struct HWControlModeAndTargetType
@@ -146,8 +147,10 @@ module armarx
long timestampUSec = 0;
};
sequence<ControlDeviceStatus> ControlDeviceStatusSeq;
-
-
+}
+//RobotUnit Utility types - SensorDevice
+module armarx
+{
struct SensorDeviceDescription
{
string deviceName;
@@ -162,15 +165,76 @@ module armarx
long timestampUSec = 0;
};
sequence<SensorDeviceStatus> SensorDeviceStatusSeq;
-
-
+}
+//RobotUnit Utility types - NJointController
+module armarx
+{
struct NJointControllerClassDescription
{
string className;
WidgetDescription::Widget configDescription;
};
sequence<NJointControllerClassDescription> NJointControllerClassDescriptionSeq;
+}
+//RobotUnit Utility types - data streaming
+module armarx
+{
+ module RobotUnitDataStreaming
+ {
+ enum DataEntryType
+ {
+ NodeTypeBool,
+ NodeTypeByte,
+ NodeTypeShort,
+ NodeTypeInt,
+ NodeTypeLong,
+ NodeTypeFloat,
+ NodeTypeDouble
+ };
+
+ struct DataEntry
+ {
+ DataEntryType type;
+ long index = -1;
+ };
+ dictionary<string, DataEntry> StringDataEntryMap;
+
+ struct DataStreamingDescription
+ {
+ StringDataEntryMap entries;
+ };
+
+ struct TimeStep
+ {
+ long iterationId;
+ long timestampUSec;
+ long timesSinceLastIterationUSec;
+ Ice::BoolSeq bools;
+ Ice::ByteSeq bytes;
+ Ice::ShortSeq shorts;
+ Ice::IntSeq ints;
+ Ice::LongSeq longs;
+ Ice::FloatSeq floats;
+ Ice::DoubleSeq doubles;
+ };
+ sequence<TimeStep> TimeStepSeq;
+
+ interface Receiver
+ {
+ void update(TimeStepSeq data);
+ };
+
+ struct Config
+ {
+ Ice::StringSeq loggingNames;
+ };
+ }
+}
+
+//RobotUnit Listener
+module armarx
+{
interface RobotUnitListener
{
void nJointControllerStatusChanged(NJointControllerStatusSeq status);
@@ -180,7 +244,11 @@ module armarx
void nJointControllerCreated(string instanceName);
void nJointControllerDeleted(string instanceName);
};
+}
+//RobotUnit Modules
+module armarx
+{
module RobotUnitModule
{
interface RobotUnitManagementInterface extends AggregatedRobotHealthInterface
@@ -199,6 +267,9 @@ module armarx
void stopRtLogging(RemoteReferenceCounterBase token) throws LogicError;
["cpp:const"] void writeRecentIterationsToFile(string filePathFormatString) throws LogicError, InvalidArgumentException;
+
+ RobotUnitDataStreaming::DataStreamingDescription startDataStreaming(RobotUnitDataStreaming::Receiver* receiver, RobotUnitDataStreaming::Config config);
+ void stopDataStreaming(RobotUnitDataStreaming::Receiver* receiver);
};
interface RobotUnitUnitInterface
{
@@ -210,9 +281,6 @@ module armarx
["cpp:const"] idempotent PlatformUnitInterface* getPlatformUnit();
["cpp:const"] idempotent TCPControlUnitInterface* getTCPControlUnit();
["cpp:const"] idempotent TrajectoryPlayerInterface* getTrajectoryPlayer();
-
-
-
};
interface RobotUnitPublishingInterface
{
@@ -302,16 +370,20 @@ module armarx
["cpp:const"] idempotent EmergencyStopState getRtEmergencyStopState();
};
};
+}
+//RobotUnit
+module armarx
+{
interface RobotUnitInterface extends
- RobotUnitModule::RobotUnitUnitInterface,
- RobotUnitModule::RobotUnitDevicesInterface,
- RobotUnitModule::RobotUnitLoggingInterface,
- RobotUnitModule::RobotUnitPublishingInterface,
- RobotUnitModule::RobotUnitManagementInterface,
- RobotUnitModule::RobotUnitControlThreadInterface,
- RobotUnitModule::RobotUnitControllerManagementInterface,
- RobotUnitModule::RobotUnitSelfCollisionCheckerInterface
+ RobotUnitModule::RobotUnitUnitInterface,
+ RobotUnitModule::RobotUnitDevicesInterface,
+ RobotUnitModule::RobotUnitLoggingInterface,
+ RobotUnitModule::RobotUnitPublishingInterface,
+ RobotUnitModule::RobotUnitManagementInterface,
+ RobotUnitModule::RobotUnitControlThreadInterface,
+ RobotUnitModule::RobotUnitControllerManagementInterface,
+ RobotUnitModule::RobotUnitSelfCollisionCheckerInterface
{
};
};