diff --git a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
index 58b688557eb86e8efb28afd0329d8f3b50e8508c..94dc94f8b1b0763dcf0e05e8dae75c05aa261896 100644
--- a/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
+++ b/source/RobotAPI/components/armem/server/RobotStateMemory/RobotUnitReader.cpp
@@ -116,12 +116,13 @@ namespace armarx::armem::server::robot_state
for (const auto& [nameEntry, dataEntry] : description.entries)
{
std::string name = nameEntry;
- std::string jointOrWhateverName;
+ std::string jointOrWhateverName = nameEntry;
std::string groupName = "";
if (auto it = configSensorMapping.find(name); it != configSensorMapping.end())
{
groupName = it->second;
+ jointOrWhateverName = it->second; // ???
}
else
{
@@ -134,10 +135,9 @@ namespace armarx::armem::server::robot_state
<< "and Y is the actual sensor. Ignoring this sensor.";
continue;
}
- groupName = name.substr(0, second_dot_pos);
+ groupName = name.substr(0, second_dot_pos);
jointOrWhateverName = name.substr(first_dot_pos + 1, second_dot_pos - first_dot_pos - 1);
-
name = name.substr(second_dot_pos + 1); // remove the groupName, TODO check if +1 is valid
}
@@ -161,7 +161,8 @@ namespace armarx::armem::server::robot_state
}
RobotUnitData::RobotUnitDataGroup& group = it->second;
- group.data->addElement(name, RobotUnitDataStreaming::toAron(data.value(), dataEntry));
+ const std::string escapedName = simox::alg::replace_all(name, ".", "/");
+ group.data->addElement(escapedName, RobotUnitDataStreaming::toAron(data.value(), dataEntry));
}
auto stop = std::chrono::high_resolution_clock::now();
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index fbfe25bdd323e2bd073c623779a9397903853602..5498ee1d8538633506bb7045128f5cf150ac1b55 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -91,20 +91,18 @@ namespace armarx
ARMARX_INFO_S << "Start initialising scanner [Ip: " << this->ip
<< "] [Port: " << this->port << "]";
// attempt to connect/reconnect
- delete this->scanner; // disconnect scanner
- if (this->useTcp)
+ if (this->scanner)
{
- this->scanner = new SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, 'A');
+ ARMARX_WARNING_S << "Scanner already initized.";
+ delete this->scanner; // disconnect scanner
}
- else
- {
- ARMARX_ERROR_S << "TCP is not switched on. Probably hostname or port not set.\n";
- return;
- }
- result = this->scanner->init();
+
+ this->scanner = new SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, 'A');
+
+ this->result = this->scanner->init();
- if (result == sick_scan::ExitSuccess) // OK -> loop again
+ if (this->result == sick_scan::ExitSuccess) // OK -> loop again
{
this->isSensorInitialized = true;
ARMARX_INFO_S << "Scanner initialized.";
@@ -126,12 +124,12 @@ namespace armarx
// def->topic(myTopicListener);
// Subscribe to a topic (passing the topic name).
- def->topic<LaserScannerUnitListenerPrx>(properties.topicName);
+ //def->topic<LaserScannerUnitListenerPrx>(properties.topicName);
// Use (and depend on) another component (passing the ComponentInterfacePrx).
// def->component(myComponentProxy)
- def->optional(properties.topicName, "topicName", "Name of the topic");
+ def->topic(topic, properties.topicName, "TopicName", "Name of the laserscanner topic to report to.");
//Scanner parameters
def->optional(properties.devices, "devices", "List of Devices in format frame1,ip1,port1;frame2,ip2,port2");
def->optional(properties.scannerType, "scannerType", "Name of the LaserScanner");
@@ -145,8 +143,8 @@ namespace armarx
{
// Topics and properties defined above are automagically registered.
- offeringTopic(properties.topicName);
- ARMARX_INFO_S << "SickLaserUnit is going to report on topic " << properties.topicName;
+ //offeringTopic(properties.topicName);
+ //ARMARX_INFO_S << "SickLaserUnit is going to report on topic " << properties.topicName;
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
@@ -169,21 +167,23 @@ namespace armarx
device.frameName = deviceInfo[0];
if (deviceInfo[1] != "")
{
- device.useTcp = true;
device.ip = deviceInfo[1];
}
+ else
+ {
+ ARMARX_FATAL << "TCP is not switched on. Probably hostname or port not set.";
+ return;
+
+ }
device.port = deviceInfo[2];
- device.scannerType = properties.scannerType;
device.timelimit = properties.timelimit;
- device.rangeMin = properties.rangeMin;
- device.rangeMax = properties.rangeMax;
//scanInfo
device.scanInfo.device = device.ip;
device.scanInfo.frame = device.frameName;
//scanner Parameters
try
{
- device.parser = new sick_scan::SickGenericParser(device.scannerType);
+ device.parser = new sick_scan::SickGenericParser(properties.scannerType);
device.parser->set_range_min(properties.rangeMin);
device.parser->set_range_max(properties.rangeMax);
device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
@@ -201,7 +201,7 @@ namespace armarx
void SickLaserUnit::onConnectComponent()
{
- topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
+ //topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
for (SickLaserScanDevice& device : scanDevices)
{
@@ -209,6 +209,7 @@ namespace armarx
//start the laser scanner
if (device.task)
{
+ ARMARX_WARNING << "this should not happen.";
device.task->stop();
device.task = nullptr;
}
@@ -240,6 +241,14 @@ namespace armarx
device.task->stop();
device.task = nullptr;
}
+ }
+ }
+
+ void SickLaserUnit::onExitComponent()
+ {
+ for (SickLaserScanDevice& device : scanDevices)
+ {
+
if (device.scanner)
{
delete device.scanner;
@@ -249,9 +258,9 @@ namespace armarx
delete device.parser;
}
}
- }
- void SickLaserUnit::onExitComponent() {}
+
+ }
std::string SickLaserUnit::getDefaultName() const
{
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 8ea76934d685f3b3a7b078dc76d0cb3250a699f8..f6ffc67a1ad3992967c8a4526dd6640b21caa4fb 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -54,15 +54,10 @@ namespace armarx
struct SickLaserScanDevice
{
- //scanner parameters
- std::string scannerType = "sick_tim_5xx";
//communication parameters
std::string ip;
std::string port;
int timelimit = 5;
- double rangeMin = 0.0;
- double rangeMax = 10.0;
- bool useTcp = false;
//data and task pointers
IceUtil::Time scanTime;
LaserScan scanData;
@@ -73,8 +68,8 @@ namespace armarx
std::string frameName = "LaserScannerFront";
LaserScannerUnitListenerPrx scanTopic;
//scanner pointers
- sick_scan::SickGenericParser* parser;
- SickScanAdapter* scanner;
+ sick_scan::SickGenericParser* parser = 0;
+ SickScanAdapter* scanner = 0;
int result = sick_scan::ExitError;
bool isSensorInitialized = false;
@@ -132,7 +127,6 @@ namespace armarx
//scanner parameters
std::string devices = "LaserScannerFront,192.168.8.133,2112";
std::string scannerType = "sick_tim_5xx";
- ScanProtocol protocol = ScanProtocol::ASCII;
int timelimit = 5;
double rangeMin = 0.0;
double rangeMax = 10.0;
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 54f94e7eb508f995585650332904c53043b5c7ee..69d33ced49a00c415f7e08456d21177c27dfb8b6 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -187,442 +187,37 @@ namespace armarx
while (dataToProcess)
{
- /*
- if (useBinaryProtocol)
- {
- // if binary protocol used then parse binary message
- std::vector<unsigned char> receiveBufferVec = std::vector<unsigned char>(receiveBuffer,
- receiveBuffer + actual_length);
- if (receiveBufferVec.size() > 8)
+ size_t dlength;
+ int success = -1;
+ // Always Parsing Ascii-Encoding of datagram
+ dstart = strchr(buffer_pos, 0x02);
+ if (dstart != NULL)
{
- long idVal = 0;
- long lenVal = 0;
- memcpy(&idVal, receiveBuffer + 0, 4); // read identifier
- memcpy(&lenVal, receiveBuffer + 4, 4); // read length indicator
- swap_endian((unsigned char *) &lenVal, 4);
-
- if (idVal == 0x02020202) // id for binary message
- {
- // binary message
- if (lenVal < actual_length)
- {
- short elevAngleX200 = 0; // signed short (F5 B2 -> Layer 24
- // F5B2h -> -2638/200= -13.19°
- int scanFrequencyX100 = 0;
- double elevAngle = 0.00;
- double scanFrequency = 0.0;
- long measurementFrequencyDiv100 = 0; // multiply with 100
- int numOfEncoders = 0;
- int numberOf16BitChannels = 0;
- int numberOf8BitChannels = 0;
- uint32_t SystemCountScan = 0;
- uint32_t SystemCountTransmit = 0;
-
- memcpy(&elevAngleX200, receiveBuffer + 50, 2);
- swap_endian((unsigned char *) &elevAngleX200, 2);
-
- memcpy(&SystemCountScan, receiveBuffer + 0x26, 4);
- swap_endian((unsigned char *) &SystemCountScan, 4);
-
- memcpy(&SystemCountTransmit, receiveBuffer + 0x2A, 4);
- swap_endian((unsigned char *) &SystemCountTransmit, 4);
-
- memcpy(&scanFrequencyX100, receiveBuffer + 52, 4);
- swap_endian((unsigned char *) &scanFrequencyX100, 4);
-
- memcpy(&measurementFrequencyDiv100, receiveBuffer + 56, 4);
- swap_endian((unsigned char *) &measurementFrequencyDiv100, 4);
-
- float scan_time = 1.0 / (scanFrequencyX100 / 100.0);
-
- //due firmware inconsistency
- if (measurementFrequencyDiv100 > 10000)
- {
- measurementFrequencyDiv100 /= 100;
- }
- float time_increment = 1.0 / (measurementFrequencyDiv100 * 100.0);
- timeIncrement = time_increment;
-
- memcpy(&numOfEncoders, receiveBuffer + 60, 2);
- swap_endian((unsigned char *) &numOfEncoders, 2);
- int encoderDataOffset = 6 * numOfEncoders;
- int32_t EncoderPosTicks[4] = {0};
- int16_t EncoderSpeed[4] = {0};
-
- if (numOfEncoders > 0 && numOfEncoders < 5)
- {
- FireEncoder = true;
- for (int EncoderNum = 0; EncoderNum < numOfEncoders; EncoderNum++)
- {
- memcpy(&EncoderPosTicks[EncoderNum], receiveBuffer + 62 + EncoderNum * 6, 4);
- swap_endian((unsigned char *) &EncoderPosTicks[EncoderNum], 4);
- memcpy(&EncoderSpeed[EncoderNum], receiveBuffer + 66 + EncoderNum * 6, 2);
- swap_endian((unsigned char *) &EncoderSpeed[EncoderNum], 2);
- }
- }
- //TODO handle multi encoder with multiple encode msg or different encoder msg definition now using only first encoder
- EncoderMsg.enc_position = EncoderPosTicks[0];
- EncoderMsg.enc_speed = EncoderSpeed[0];
- memcpy(&numberOf16BitChannels, receiveBuffer + 62 + encoderDataOffset, 2);
- swap_endian((unsigned char *) &numberOf16BitChannels, 2);
-
- int parseOff = 64 + encoderDataOffset;
-
- char szChannel[255] = {0};
- float scaleFactor = 1.0;
- float scaleFactorOffset = 0.0;
- int32_t startAngleDiv10000 = 1;
- int32_t sizeOfSingleAngularStepDiv10000 = 1;
- double startAngle = 0.0;
- double sizeOfSingleAngularStep = 0.0;
- short numberOfItems = 0;
-
- static int cnt = 0;
- cnt++;
- // get number of 8 bit channels
- // we must jump of the 16 bit data blocks including header ...
- for (int i = 0; i < numberOf16BitChannels; i++)
- {
- int numberOfItems = 0x00;
- memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
- swap_endian((unsigned char *) &numberOfItems, 2);
- parseOff += 21; // 21 Byte header followed by data entries
- parseOff += numberOfItems * 2;
- }
-
- // now we can read the number of 8-Bit-Channels
- memcpy(&numberOf8BitChannels, receiveBuffer + parseOff, 2);
- swap_endian((unsigned char *) &numberOf8BitChannels, 2);
-
- parseOff = 64 + encoderDataOffset;
- enum datagram_parse_task
- {
- process_dist,
- process_vang,
- process_rssi,
- process_idle
- };
- int rssiCnt = 0;
- int vangleCnt = 0;
- int distChannelCnt = 0;
-
- for (int processLoop = 0; processLoop < 2; processLoop++)
- {
- int totalChannelCnt = 0;
-
-
- bool bCont = true;
-
- datagram_parse_task task = process_idle;
- bool parsePacket = true;
- parseOff = 64 + encoderDataOffset;
- bool processData = false;
-
- if (processLoop == 0)
- {
- distChannelCnt = 0;
- rssiCnt = 0;
- vangleCnt = 0;
- }
-
- if (processLoop == 1)
- {
- processData = true;
- numEchos = distChannelCnt;
- ranges.resize(numberOfItems * numEchos);
- if (rssiCnt > 0)
- {
- intensities.resize(numberOfItems * rssiCnt);
- }
- else
- {
- }
- if (vangleCnt > 0) // should be 0 or 1
- {
- vang_vec.resize(numberOfItems * vangleCnt);
- }
- else
- {
- vang_vec.clear();
- }
- echoMask = (1 << numEchos) - 1;
-
- // reset count. We will use the counter for index calculation now.
- distChannelCnt = 0;
- rssiCnt = 0;
- vangleCnt = 0;
-
- }
-
- szChannel[6] = '\0';
- scaleFactor = 1.0;
- scaleFactorOffset = 0.0;
- startAngleDiv10000 = 1;
- sizeOfSingleAngularStepDiv10000 = 1;
- startAngle = 0.0;
- sizeOfSingleAngularStep = 0.0;
- numberOfItems = 0;
-
-
- #if 1 // prepared for multiecho parsing
-
- bCont = true;
- bool doVangVecProc = false;
- // try to get number of DIST and RSSI from binary data
- task = process_idle;
- do
- {
- task = process_idle;
- doVangVecProc = false;
- int processDataLenValuesInBytes = 2;
-
- if (totalChannelCnt == numberOf16BitChannels)
- {
- parseOff += 2; // jump of number of 8 bit channels- already parsed above
- }
-
- if (totalChannelCnt >= numberOf16BitChannels)
- {
- processDataLenValuesInBytes = 1; // then process 8 bit values ...
- }
- bCont = false;
- strcpy(szChannel, "");
-
- if (totalChannelCnt < (numberOf16BitChannels + numberOf8BitChannels))
- {
- szChannel[5] = '\0';
- strncpy(szChannel, (const char *) receiveBuffer + parseOff, 5);
- }
- else
- {
- // all channels processed (16 bit and 8 bit channels)
- }
-
- if (strstr(szChannel, "DIST") == szChannel)
- {
- task = process_dist;
- distChannelCnt++;
- bCont = true;
- numberOfItems = 0;
- memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
- swap_endian((unsigned char *) &numberOfItems, 2);
-
- }
- if (strstr(szChannel, "VANG") == szChannel)
- {
- vangleCnt++;
- task = process_vang;
- bCont = true;
- numberOfItems = 0;
- memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
- swap_endian((unsigned char *) &numberOfItems, 2);
-
- vang_vec.resize(numberOfItems);
-
- }
- if (strstr(szChannel, "RSSI") == szChannel)
- {
- task = process_rssi;
- rssiCnt++;
- bCont = true;
- numberOfItems = 0;
- // copy two byte value (unsigned short to numberOfItems
- memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
- swap_endian((unsigned char *) &numberOfItems, 2); // swap
-
- }
- if (bCont)
- {
- scaleFactor = 0.0;
- scaleFactorOffset = 0.0;
- startAngleDiv10000 = 0;
- sizeOfSingleAngularStepDiv10000 = 0;
- numberOfItems = 0;
-
- memcpy(&scaleFactor, receiveBuffer + parseOff + 5, 4);
- memcpy(&scaleFactorOffset, receiveBuffer + parseOff + 9, 4);
- memcpy(&startAngleDiv10000, receiveBuffer + parseOff + 13, 4);
- memcpy(&sizeOfSingleAngularStepDiv10000, receiveBuffer + parseOff + 17, 2);
- memcpy(&numberOfItems, receiveBuffer + parseOff + 19, 2);
-
-
- swap_endian((unsigned char *) &scaleFactor, 4);
- swap_endian((unsigned char *) &scaleFactorOffset, 4);
- swap_endian((unsigned char *) &startAngleDiv10000, 4);
- swap_endian((unsigned char *) &sizeOfSingleAngularStepDiv10000, 2);
- swap_endian((unsigned char *) &numberOfItems, 2);
-
- if (processData)
- {
- unsigned short *data = (unsigned short *) (receiveBuffer + parseOff + 21);
-
- unsigned char *swapPtr = (unsigned char *) data;
- // copy RSSI-Values +2 for 16-bit values +1 for 8-bit value
- for (int i = 0;
- i < numberOfItems * processDataLenValuesInBytes; i += processDataLenValuesInBytes)
- {
- if (processDataLenValuesInBytes == 1)
- {
- }
- else
- {
- unsigned char tmp;
- tmp = swapPtr[i + 1];
- swapPtr[i + 1] = swapPtr[i];
- swapPtr[i] = tmp;
- }
- }
- int idx = 0;
-
- switch (task)
- {
-
- case process_dist:
- {
- startAngle = startAngleDiv10000 / 10000.00;
- sizeOfSingleAngularStep = sizeOfSingleAngularStepDiv10000 / 10000.0;
- sizeOfSingleAngularStep *= (M_PI / 180.0);
-
- msg.angle_min = startAngle / 180.0 * M_PI - M_PI / 2;
- msg.angle_increment = sizeOfSingleAngularStep;
- msg.angle_max = msg.angle_min + (numberOfItems - 1) * msg.angle_increment;
-
- if (this->parser_->getCurrentParamPtr()->getScanMirroredAndShifted())
- {
- msg.angle_min -= M_PI/2;
- msg.angle_max -= M_PI/2;
-
- msg.angle_min *= -1.0;
- msg.angle_increment *= -1.0;
- msg.angle_max *= -1.0;
-
- }
- float *rangePtr = NULL;
-
- if (numberOfItems > 0)
- {
- rangePtr = &msg.ranges[0];
- }
- float scaleFactor_001 = 0.001F * scaleFactor;// to avoid repeated multiplication
- for (int i = 0; i < numberOfItems; i++)
- {
- idx = i + numberOfItems * (distChannelCnt - 1);
- rangePtr[idx] = (float) data[i] * scaleFactor_001 + scaleFactorOffset;
- #ifdef DEBUG_DUMP_ENABLED
- if (distChannelCnt == 1)
- {
- if (i == floor(numberOfItems / 2))
- {
- double curTimeStamp = SystemCountScan + i * msg.time_increment * 1E6;
- //DataDumper::instance().pushData(curTimeStamp, "DIST", rangePtr[idx]);
- }
- }
- #endif
- //XXX
- }
-
- }
- break;
- case process_rssi:
- {
- // Das muss vom Protokoll abgeleitet werden. !!!
-
- float *intensityPtr = NULL;
-
- if (numberOfItems > 0)
- {
- intensityPtr = &msg.intensities[0];
-
- }
- for (int i = 0; i < numberOfItems; i++)
- {
- idx = i + numberOfItems * (rssiCnt - 1);
- // we must select between 16 bit and 8 bit values
- float rssiVal = 0.0;
- if (processDataLenValuesInBytes == 2)
- {
- rssiVal = (float) data[i];
- }
- else
- {
- unsigned char *data8Ptr = (unsigned char *) data;
- rssiVal = (float) data8Ptr[i];
- }
- intensityPtr[idx] = rssiVal * scaleFactor + scaleFactorOffset;
- }
- }
- break;
-
- case process_vang:
- float *vangPtr = NULL;
- if (numberOfItems > 0)
- {
- vangPtr = &vang_vec[0]; // much faster, with vang_vec[i] each time the size will be checked
- }
- for (int i = 0; i < numberOfItems; i++)
- {
- vangPtr[i] = (float) data[i] * scaleFactor + scaleFactorOffset;
- }
- break;
- }
- }
- parseOff += 21 + processDataLenValuesInBytes * numberOfItems;
-
-
- }
- totalChannelCnt++;
- } while (bCont);
- }
- #endif
-
- elevAngle = elevAngleX200 / 200.0;
- scanFrequency = scanFrequencyX100 / 100.0;
-
-
- }
- }
+ dend = strchr(dstart + 1, 0x03);
}
-
- success = sick_scan::ExitSuccess;
- // change Parsing Mode
- dataToProcess = false; // only one package allowed - no chaining
- }
- else // Parsing of Ascii-Encoding of datagram, xxx
- */
+ if ((dstart != NULL) && (dend != NULL))
{
-
- size_t dlength;
- int success = -1;
- // Always Parsing Ascii-Encoding of datagram
- dstart = strchr(buffer_pos, 0x02);
- if (dstart != NULL)
- {
- dend = strchr(dstart + 1, 0x03);
- }
- if ((dstart != NULL) && (dend != NULL))
- {
- dataToProcess = true; // continue parsing
- dlength = dend - dstart;
- *dend = '\0';
- dstart++;
- }
- else
- {
- dataToProcess = false;
- break;
- }
- // HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
- // <frameid>_<sign>00500_DIST[1|2|3]
- success = parseDatagram(dstart, dlength, scanData, scanInfo, updateScannerInfo);
- if (success != sick_scan::ExitSuccess)
- {
- ARMARX_WARNING << "parseDatagram returned ErrorCode: " << success;
- }
- // Start Point
- if (dend != NULL)
- {
- buffer_pos = dend + 1;
- }
+ dataToProcess = true; // continue parsing
+ dlength = dend - dstart;
+ *dend = '\0';
+ dstart++;
+ }
+ else
+ {
+ dataToProcess = false;
+ break;
+ }
+ // HEADER of data followed by DIST1 ... DIST2 ... DIST3 .... RSSI1 ... RSSI2.... RSSI3...
+ // <frameid>_<sign>00500_DIST[1|2|3]
+ success = parseDatagram(dstart, dlength, scanData, scanInfo, updateScannerInfo);
+ if (success != sick_scan::ExitSuccess)
+ {
+ ARMARX_WARNING << "parseDatagram returned ErrorCode: " << success;
+ }
+ // Start Point
+ if (dend != NULL)
+ {
+ buffer_pos = dend + 1;
}
} // end of while loop
return sick_scan::ExitSuccess; // return success to continue looping
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
index a4ca9b08ad6d3b0937294c5a9050adb6680abfcc..594e05ed14002d7098c2ecaa41a5d627e3f9ea24 100644
--- a/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/CMakeLists.txt
@@ -25,6 +25,7 @@ set(LIB_FILES
NaturalIKComponentPlugin.cpp
HandUnitComponentPlugin.cpp
FrameTrackingComponentPlugin.cpp
+ HeartbeatComponentPlugin.cpp
)
set(LIB_HEADERS
RobotStateComponentPlugin.h
@@ -40,6 +41,7 @@ set(LIB_HEADERS
NaturalIKComponentPlugin.h
HandUnitComponentPlugin.h
FrameTrackingComponentPlugin.h
+ HeartbeatComponentPlugin.h
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..510ff8933b6e045ec96e1020e6914f2b064636f2
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.cpp
@@ -0,0 +1,98 @@
+#include "HeartbeatComponentPlugin.h"
+
+#include "ArmarXCore/core/Component.h"
+#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
+
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
+
+namespace armarx::plugins
+{
+ void
+ HeartbeatComponentPlugin::configureHeartbeatChannel(const std::string& channel,
+ const RobotHealthHeartbeatArgs& args)
+ {
+ channelHeartbeatConfig.emplace(channel, args);
+ }
+
+ void
+ HeartbeatComponentPlugin::heartbeat()
+ {
+
+ if (robotHealthTopic)
+ {
+ robotHealthTopic->heartbeat(componentName, heartbeatArgs);
+ }
+ else
+ {
+ ARMARX_WARNING << "No robot health topic available!";
+ }
+ }
+
+ void
+ HeartbeatComponentPlugin::heartbeat(const std::string& channel)
+ {
+ const auto argsIt = channelHeartbeatConfig.find(channel);
+ ARMARX_CHECK(argsIt != channelHeartbeatConfig.end())
+ << "heartbeat() called for unknown channel '" << channel << "'."
+ << "You must register the config using configureHeartbeatChannel(channel) first!";
+
+ const auto& args = argsIt->second;
+
+ if (robotHealthTopic)
+ {
+ robotHealthTopic->heartbeat(componentName + "_" + channel, args);
+ }
+ else
+ {
+ ARMARX_WARNING << "No robot health topic available!";
+ }
+ }
+
+ void
+ HeartbeatComponentPlugin::preOnInitComponent()
+ {
+ if (topicName.empty())
+ {
+ parent<Component>().getProperty(topicName, makePropertyName(topicPropertyName));
+ }
+ parent<Component>().offeringTopic(topicName);
+ }
+
+ void
+ HeartbeatComponentPlugin::postOnInitComponent()
+ {
+ }
+
+ void
+ HeartbeatComponentPlugin::preOnConnectComponent()
+ {
+ robotHealthTopic = parent<Component>().getTopic<RobotHealthInterfacePrx>(topicName);
+ }
+
+ void
+ HeartbeatComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
+ {
+ if (!properties->hasDefinition(makePropertyName(topicPropertyName)))
+ {
+ properties->defineOptionalProperty<std::string>(
+ makePropertyName(topicPropertyName),
+ "DebugObserver",
+ "Name of the topic the DebugObserver listens on");
+ }
+
+ if (not properties->hasDefinition(makePropertyName(maximumCycleTimeWarningMSPropertyName)))
+ {
+ properties->defineRequiredProperty<std::string>(
+ makePropertyName(maximumCycleTimeWarningMSPropertyName),
+ "TODO: maximumCycleTimeWarningMS");
+ }
+
+ if (not properties->hasDefinition(makePropertyName(maximumCycleTimeErrorMSPropertyName)))
+ {
+ properties->defineRequiredProperty<std::string>(
+ makePropertyName(maximumCycleTimeErrorMSPropertyName),
+ "TODO: maximumCycleTimeErrorMS");
+ }
+ }
+
+} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..6a4a6c2bd692484c94bbd42bdfff399b0f5a0aa9
--- /dev/null
+++ b/source/RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h
@@ -0,0 +1,89 @@
+/**
+ * This file is part of ArmarX.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author Fabian Reister ( fabian dot reister at kit dot edu )
+ * @date 2021
+ * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+ * GNU General Public License
+ */
+
+#pragma once
+
+#include <ArmarXCore/core/ComponentPlugin.h>
+
+#include <RobotAPI/interface/components/RobotHealthInterface.h>
+
+namespace armarx::plugins
+{
+
+ class HeartbeatComponentPlugin : public ComponentPlugin
+ {
+ public:
+ using ComponentPlugin::ComponentPlugin;
+
+ /**
+ * @brief Configures a heartbeat subchannel.
+ *
+ * @param channel Identifier of the heartbeat channel
+ * @param args Configuration of this channel's heartbeat properties
+ */
+ void configureHeartbeatChannel(const std::string& channel,
+ const RobotHealthHeartbeatArgs& args);
+
+ /**
+ * @brief Sends out a heartbeat using the default config
+ *
+ */
+ void heartbeat();
+
+ /**
+ * @brief Sends out a heartbeat for a subchannel.
+ *
+ * Note: You must call configureHeartbeatChannel(...) first to register the channel config!
+ *
+ * @param channel Identifier of the heartbeat channel
+ */
+ void heartbeat(const std::string& channel);
+
+ protected:
+ void preOnInitComponent() override;
+ void postOnInitComponent() override;
+ void preOnConnectComponent() override;
+
+ void postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties) override;
+
+ private:
+ //! heartbeat topic name (outgoing)
+ std::string topicName;
+
+ //! name of this component used as identifier for heartbeats
+ std::string componentName;
+
+ //
+ static constexpr auto topicPropertyName = "heartbeat.TopicName";
+ static constexpr auto maximumCycleTimeWarningMSPropertyName =
+ "heartbeat.maximumCycleTimeWarningMS";
+ static constexpr auto maximumCycleTimeErrorMSPropertyName =
+ "heartbeat.maximumCycleTimeErrorMS";
+
+ RobotHealthInterfacePrx robotHealthTopic;
+
+ //! default config used in heartbeat(), set via properties
+ RobotHealthHeartbeatArgs heartbeatArgs;
+
+ //! configs used in heartbeat(channel), set by user via configureHeartbeatChannel(...)
+ std::unordered_map<std::string, RobotHealthHeartbeatArgs> channelHeartbeatConfig;
+ };
+} // namespace armarx::plugins
diff --git a/source/RobotAPI/libraries/armem/client/Query.h b/source/RobotAPI/libraries/armem/client/Query.h
index 916278a8fcde1679545ed39d59531f5772852c13..c6bb445bfe8ee9a3547ae621aa8c16ec8e0e2be9 100644
--- a/source/RobotAPI/libraries/armem/client/Query.h
+++ b/source/RobotAPI/libraries/armem/client/Query.h
@@ -60,6 +60,38 @@ namespace armarx::armem::client
}
}
+ void replaceQueryTarget(const armem::query::data::QueryTarget search, const armem::query::data::QueryTarget replace)
+ {
+ for (const auto& memoryQuery : memoryQueries)
+ {
+ if (auto it = std::find(memoryQuery->targets.begin(), memoryQuery->targets.end(), search); it != memoryQuery->targets.end())
+ {
+ memoryQuery->targets.push_back(replace);
+ }
+ for (const auto& coreSegmentQuery : memoryQuery->coreSegmentQueries)
+ {
+ if (auto it = std::find(coreSegmentQuery->targets.begin(), coreSegmentQuery->targets.end(), search); it != coreSegmentQuery->targets.end())
+ {
+ coreSegmentQuery->targets.push_back(replace);
+ }
+ for (const auto& providerSegmentQuery : coreSegmentQuery->providerSegmentQueries)
+ {
+ if (auto it = std::find(providerSegmentQuery->targets.begin(), providerSegmentQuery->targets.end(), search); it != providerSegmentQuery->targets.end())
+ {
+ providerSegmentQuery->targets.push_back(replace);
+ }
+ for (const auto& entityQuery : providerSegmentQuery->entityQueries)
+ {
+ if (auto it = std::find(entityQuery->targets.begin(), entityQuery->targets.end(), search); it != entityQuery->targets.end())
+ {
+ entityQuery->targets.push_back(replace);
+ }
+ }
+ }
+ }
+ }
+ }
+
static QueryInput fromIce(const armem::query::data::Input& ice);
armem::query::data::Input toIce() const;
};
diff --git a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
index 781322b5493388f2e88d5b0b7d20242b79048ca4..1bc42b0cc579dbf8abe4f2831608166451ff8fbb 100644
--- a/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
+++ b/source/RobotAPI/libraries/armem/core/base/MemoryBase.h
@@ -336,6 +336,25 @@ namespace armarx::armem::base
return this->name();
}
+ std::string dump() const
+ {
+ std::stringstream ss;
+ ss << "Memory: " << this->name() << "\n";
+ for (const auto& [ckey, cseg] : this->container())
+ {
+ ss << " |- Found core seg: " << ckey << "\n";
+ for (const auto& [pkey, pseg] : cseg.container())
+ {
+ ss << " | |- Found prov seg: " << pkey << "\n";
+ for (const auto& [ekey, eseg] : pseg.container())
+ {
+ ss << " | | |- Found entity: " << ekey << "\n";
+ }
+ }
+ }
+ return ss.str();
+ }
+
protected:
virtual void _copySelf(DerivedT& other) const override
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
index a64a5982e143e2627718f5039d3559b2d3479348..d47dc46cb63c5266edc8b9e8457c1fb4b75a4527 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/CoreSegment.cpp
@@ -10,8 +10,6 @@ namespace armarx::armem::ltm
wm::CoreSegment CoreSegment::convert() const
{
- ARMARX_INFO << "CoreSegment: Converting with connection to: " << dbsettings.toString();
-
wm::CoreSegment m(id());
for (const auto& [_, s] : _container)
{
@@ -24,8 +22,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "CoreSegment: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
@@ -47,7 +43,7 @@ namespace armarx::armem::ltm
if (const auto& it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (core) container a key k = " + k + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -56,6 +52,8 @@ namespace armarx::armem::ltm
wms.first->second.reload();
}
}
+
+ ARMARX_INFO << "After reload has core segment " << id().str() << " size: " << _container.size();
}
void CoreSegment::append(const wm::CoreSegment& m)
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
index aeb2be76e6ae6951188af4174b9b11dc51d736fc..bccb79c9d1d162a6c67a4f80c57b58b04425863e 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Entity.cpp
@@ -6,8 +6,6 @@ namespace armarx::armem::ltm
wm::Entity Entity::convert() const
{
- ARMARX_INFO << "Entity: Converting with connection to: " << dbsettings.toString();
-
wm::Entity m(id());
for (const auto& [_, s] : _container)
{
@@ -20,8 +18,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "Entity: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
@@ -42,7 +38,7 @@ namespace armarx::armem::ltm
if (const auto& it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + std::to_string(k.toMicroSeconds()) + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (entity) container a key k = " + std::to_string(k.toMicroSeconds()) + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -52,6 +48,8 @@ namespace armarx::armem::ltm
}
++i;
}
+
+ ARMARX_INFO << "After reload has entity " << id().str() << " size: " << _container.size();
}
void Entity::append(const wm::Entity& m)
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
index 47a680731f9794c99433a114dd94a9ad22f5cbea..aae8a6d3258c94a27e9f931838a44c8d1b6426aa 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/EntitySnapshot.cpp
@@ -13,8 +13,6 @@ namespace armarx::armem::ltm
wm::EntitySnapshot EntitySnapshot::convert(const aron::typenavigator::NavigatorPtr& expectedStructure) const
{
- ARMARX_INFO << "EntitySnapshot: Converting with connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().getEntityID().str()];
@@ -52,8 +50,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "EntitySnapshot: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().getEntityID().str()];
@@ -68,10 +64,8 @@ namespace armarx::armem::ltm
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(*res));
for (unsigned int i = 0; i < json.at("instances").size(); ++i)
{
- auto wms = _container.emplace_back(id().withInstanceIndex(i));
+ _container.emplace_back(id().withInstanceIndex(i));
}
-
- ARMARX_INFO << "Entity '" + id().str() + "': Found instances in LTM: " << json.at("instances").size();
}
void EntitySnapshot::setTo(const wm::EntitySnapshot& m, const armem::Time& t)
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
index 95ad5af75460d3f106b81f5fcc9d4c11ebb2038d..2080879c55f1dd94e5aeeb2f6d78f72ada5c4aaf 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
@@ -27,7 +27,8 @@ namespace armarx::armem::ltm
dbsettings(other.dbsettings),
alwaysTransferSettings(other.alwaysTransferSettings),
periodicTransferSettings(other.periodicTransferSettings),
- onFullTransferSettings(other.onFullTransferSettings)
+ onFullTransferSettings(other.onFullTransferSettings),
+ reloaded(other.reloaded)
{
// Do not move _mutex.
}
@@ -53,6 +54,7 @@ namespace armarx::armem::ltm
alwaysTransferSettings = std::move(other.alwaysTransferSettings);
periodicTransferSettings = std::move(other.periodicTransferSettings);
onFullTransferSettings = std::move(other.onFullTransferSettings);
+ reloaded = other.reloaded;
// Don't move _mutex.
return *this;
@@ -88,13 +90,14 @@ namespace armarx::armem::ltm
wm::Memory Memory::convert() const
{
+ std::lock_guard l(mongoDBMutex);
if (!checkConnection())
{
wm::Memory m(id());
return m;
}
- ARMARX_INFO << "Converting with connection to: " << dbsettings.toString();
+ ARMARX_INFO << "Converting Memory with connection to: " << dbsettings.toString();
TIMING_START(LTM_Convert);
@@ -110,18 +113,20 @@ namespace armarx::armem::ltm
void Memory::reload()
{
+ std::lock_guard l(mongoDBMutex);
+ reloaded = false;
+
if (!checkConnection())
{
return;
}
- std::scoped_lock l(mongoDBMutex);
ARMARX_INFO << "(Re)Establishing connection to: " << dbsettings.toString();
+
+ TIMING_START(LTM_Reload);
_container.clear();
- ARMARX_TRACE;
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
- ARMARX_TRACE;
if (!client)
{
ARMARX_ERROR << "A client has died. Could not reload.";
@@ -139,6 +144,8 @@ namespace armarx::armem::ltm
ARMARX_INFO << "Getting collection for id: " << id().str();
mongocxx::collection coll = db[id().str()];
+ ARMARX_IMPORTANT << "Memory Container size is: " << _container.size();
+
mongocxx::cursor cursor = coll.find({});
for (const auto& doc : cursor)
{
@@ -155,7 +162,7 @@ namespace armarx::armem::ltm
if (const auto& it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (memory) container a key k = " + k + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -164,17 +171,26 @@ namespace armarx::armem::ltm
wms.first->second.reload();
}
}
+
+ reloaded = true;
+ for (const auto& m : toAppendQueue)
+ {
+ _append(m);
+ }
+
+ TIMING_END(LTM_Reload);
+ ARMARX_INFO << "After reload memory " << id().str() << " size: " << _container.size() << ". Setting reloaded: " << reloaded;
}
- void Memory::append(const wm::Memory& m)
+ void Memory::_append(const wm::Memory& m)
{
- if (!checkConnection())
+ if (!checkConnection() || !reloaded)
{
+ // We ignore if not fully loaded yet
return;
}
- std::scoped_lock l(mongoDBMutex);
- ARMARX_INFO << "Merge memory with name '" << m.name() << "' into the LTM with name '" << name() << "'";
+ //ARMARX_INFO << "Merge memory with name '" << m.name() << "' into the LTM with name '" << name() << "'";
TIMING_START(LTM_Append);
@@ -205,4 +221,16 @@ namespace armarx::armem::ltm
TIMING_END(LTM_Append);
}
+
+ void Memory::append(const wm::Memory& m)
+ {
+ std::lock_guard l(mongoDBMutex);
+ if (!checkConnection() || !reloaded)
+ {
+ toAppendQueue.push_back(m);
+ return;
+ }
+
+ _append(m);
+ }
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
index 43b67c82ca91a09e875f1c3f85949a45e4f2c83f..b24f4a7a662e2b5980d3dec00ecbb39b2c972673 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
@@ -87,6 +87,8 @@ namespace armarx::armem::ltm
private:
bool checkConnection() const;
+ void _append(const wm::Memory&);
+
public:
MongoDBConnectionManager::MongoDBSettings dbsettings;
@@ -95,8 +97,9 @@ namespace armarx::armem::ltm
OnFullTransferSettings onFullTransferSettings;
private:
+ bool reloaded = false;
mutable std::mutex mongoDBMutex;
-
+ std::vector<wm::Memory> toAppendQueue;
};
}
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
index aac80459904cae59550ecdf33f7057f92ed03aa4..5fcf4a1351e34f257e773c2ac32e9d4eb64c5401 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/ProviderSegment.cpp
@@ -10,8 +10,6 @@ namespace armarx::armem::ltm
wm::ProviderSegment ProviderSegment::convert() const
{
- ARMARX_INFO << "ProviderSegment: Converting with connection to: " << dbsettings.toString();
-
wm::ProviderSegment m(id());
for (const auto& [_, s] : _container)
{
@@ -24,8 +22,6 @@ namespace armarx::armem::ltm
{
_container.clear();
- ARMARX_INFO << "ProviderSegment: (Re)Establishing connection to: " << dbsettings.toString();
-
mongocxx::client& client = MongoDBConnectionManager::EstablishConnection(dbsettings);
mongocxx::database db = client[dbsettings.database];
mongocxx::collection coll = db[id().str()];
@@ -34,7 +30,6 @@ namespace armarx::armem::ltm
for (auto doc : cursor)
{
nlohmann::json json = nlohmann::json::parse(bsoncxx::to_json(doc));
- ARMARX_INFO << "ProviderSegment: Found foreign key: " << json.at("foreign_key");
MemoryID i((std::string) json.at("foreign_key"));
if (i.providerSegmentName != id().providerSegmentName)
@@ -46,7 +41,7 @@ namespace armarx::armem::ltm
if (const auto& it = _container.find(k); it != _container.end())
{
- throw error::ArMemError("Somehow after clearing the container a key k = " + k + " was found. Do you have double entries in mongodb?");
+ throw error::ArMemError("Somehow after clearing the (provvider) container a key k = " + k + " was found. Do you have double entries in mongodb?");
}
else
{
@@ -55,6 +50,8 @@ namespace armarx::armem::ltm
wms.first->second.reload();
}
}
+
+ ARMARX_INFO << "After reload has provider segment " << id().str() << " size: " << _container.size();
}
void ProviderSegment::append(const wm::ProviderSegment& m)
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 8015368837e3ea37c8e5a87837d8a3a69536e04e..6b0761f53e49c1ff65a1edf46f474d6c8cfde25b 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -173,6 +173,7 @@ namespace armarx::armem::server
result.timeArrived = update.timeArrived;
// also store in ltm if transfermode is set to always
+ // TODO: Move outside of loop?
if (longtermMemory)
{
if (longtermMemory->alwaysTransferSettings.enabled)
@@ -243,16 +244,35 @@ namespace armarx::armem::server
armem::query::data::Result result;
if (ltmResult.hasData())
{
+ ARMARX_INFO << "The LTM returned data after query";
+
// ATTENTION: This code block moves data from LTM back into WM.
// However, since some segments are constrained, the WM might send data back to LTM.
// This may also affect the data returned by the current query.
// However, this is expected behavior, since we copy the data in the processor (copyEmpty) we can safely return the copy and
// remove the original memory reference from WM here.
wm::Memory ltmConverted = ltmResult.convert();
+ if (!ltmConverted.hasData())
+ {
+ ARMARX_ERROR << "A converted memory contains no data although the original memory contained data. This indicates that something is wrong.";
+ }
+
wmResult.append(ltmConverted);
+ if (!wmResult.hasData())
+ {
+ ARMARX_ERROR << "A merged Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
+ }
// query again to limit output size (TODO: Skip if querytype is all)
- wm::Memory merged_result = wmProcessor.process(input, wmResult);
+ auto queryInput = armem::client::QueryInput::fromIce(input);
+ queryInput.replaceQueryTarget(query::data::QueryTarget::LTM, query::data::QueryTarget::WM);
+
+ wm::Memory merged_result = wmProcessor.process(queryInput.toIce(), wmResult, /* execute if: */ { query::data::QueryTarget::WM });
+ if (!merged_result.hasData())
+ {
+ ARMARX_ERROR << "A merged and postprocessed Memory has no data although at least the LTM result contains data. This indicates that something is wrong.";
+ }
+
result.memory = toIce<data::MemoryPtr>(merged_result);
// also move results of ltm to wm
@@ -263,6 +283,7 @@ namespace armarx::armem::server
}
else
{
+ ARMARX_INFO << "The LTM did not return data after query";
result.memory = toIce<data::MemoryPtr>(wmResult);
}