diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 6c84b806ffc36dca5a6e981da5d19a2a67f329d2..5498ee1d8538633506bb7045128f5cf150ac1b55 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -39,9 +39,18 @@ namespace armarx
switch (runState)
{
case RunState::scannerInit:
- initScanner();
- //read the scanner parameters for initialization
- result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
+ if (initCnt < 5)
+ {
+ initCnt++;
+ initScanner();
+ //read the scanner parameters for initialization
+ result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
+ }
+ else
+ {
+ ARMARX_WARNING << "Maximum number of reinitializations reached - going to idle state";
+ runState = RunState::scannerFinalize;
+ }
break;
case RunState::scannerRun:
if (result == sick_scan::ExitSuccess) // OK -> loop again
@@ -66,7 +75,8 @@ namespace armarx
}
break;
case RunState::scannerFinalize:
- ARMARX_WARNING << "scanner offline";
+ ARMARX_WARNING << "Scanner offline - stopping task.";
+ this->task->stop();
break;
default:
ARMARX_WARNING << "Invalid run state in task loop";
@@ -81,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.";
@@ -116,20 +124,18 @@ 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");
def->optional(properties.timelimit, "timelimit", "timelimit for communication");
- def->required(properties.scannerType, "scannerType", "Name of the LaserScanner");
- def->optional(properties.angleOffset, "angleOffset", "offset to the scanning angle");
def->optional(properties.rangeMin, "rangeMin", "minimum Range of the Scanner");
def->optional(properties.rangeMax, "rangeMax", "maximum Range of the Scanner");
- def->optional(properties.timeIncrement, "timeIncrement", "timeIncrement??");
return def;
}
@@ -137,12 +143,12 @@ 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;
// Keep debug observer data until calling `sendDebugObserverBatch()`.
// (Requies the armarx::DebugObserverComponentPluginUser.)
// setDebugObserverBatchModeEnabled(true);
- ARMARX_INFO_S << "initializing SickLaserUnit.";
-
std::vector<std::string> splitDeviceStrings = Split(properties.devices, ";");
scanDevices.clear();
scanDevices.reserve(splitDeviceStrings.size());
@@ -152,7 +158,7 @@ namespace armarx
if (deviceInfo.size() != 3)
{
ARMARX_WARNING << "Unexpected format for laser scanner device: " << deviceString
- << " (split size: " << deviceInfo.size() << ")";
+ << " (split size: " << deviceInfo.size() << ", expected: 3)";
continue;
}
SickLaserScanDevice& device = scanDevices.emplace_back();
@@ -161,41 +167,41 @@ 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.angleOffset = properties.angleOffset;
device.timelimit = properties.timelimit;
- device.scannerType = properties.scannerType;
- device.rangeMin = properties.rangeMin;
- device.rangeMax = properties.rangeMax;
- device.timeIncrement = properties.timeIncrement;
+ //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->set_time_increment(properties.timeIncrement);
+ device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
}
catch (std::exception const& e)
{
ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
return;
}
- device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
- device.colaDialectId = 'A';
- }
- //addPlugin(heartbeat);
- //configureHeartbeatChannel();
+ //addPlugin(heartbeat);
+ //configureHeartbeatChannel();
+ }
}
void SickLaserUnit::onConnectComponent()
{
- topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
- offeringTopic(properties.topicName);
+ //topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
for (SickLaserScanDevice& device : scanDevices)
{
@@ -203,6 +209,7 @@ namespace armarx
//start the laser scanner
if (device.task)
{
+ ARMARX_WARNING << "this should not happen.";
device.task->stop();
device.task = nullptr;
}
@@ -217,23 +224,9 @@ namespace armarx
// The data can be viewed in the ObserverView and the LivePlotter.
// (Before starting any threads, we don't need to lock mutexes.)
{
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- */
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- // Draw boxes in ArViz.
- // (Before starting any threads, we don't need to lock mutexes.)
- drawBoxes(properties, arviz);
- */
-
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- // Setup the remote GUI.
- {
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
+ setDebugObserverDatafield("numBoxes", properties.numBoxes);
+ setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
+ sendDebugObserverBatch();
}
*/
}
@@ -248,6 +241,14 @@ namespace armarx
device.task->stop();
device.task = nullptr;
}
+ }
+ }
+
+ void SickLaserUnit::onExitComponent()
+ {
+ for (SickLaserScanDevice& device : scanDevices)
+ {
+
if (device.scanner)
{
delete device.scanner;
@@ -257,90 +258,13 @@ namespace armarx
delete device.parser;
}
}
- }
- void SickLaserUnit::onExitComponent() {}
- std::string SickLaserUnit::getDefaultName() const
- {
- return "SickLaserUnit";
- }
-
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- void SickLaserUnit::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
-
- // Setup the widgets.
-
- tab.boxLayerName.setValue(properties.boxLayerName);
-
- tab.numBoxes.setValue(properties.numBoxes);
- tab.numBoxes.setRange(0, 100);
-
- tab.drawBoxes.setLabel("Draw Boxes");
-
- // Setup the layout.
-
- GridLayout grid;
- int row = 0;
- {
- grid.add(Label("Box Layer"), {row, 0}).add(tab.boxLayerName, {row, 1});
- ++row;
-
- grid.add(Label("Num Boxes"), {row, 0}).add(tab.numBoxes, {row, 1});
- ++row;
-
- grid.add(tab.drawBoxes, {row, 0}, {2, 1});
- ++row;
- }
-
- VBoxLayout root = {grid, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
}
-
- void SickLaserUnit::RemoteGui_update()
- {
- if (tab.boxLayerName.hasValueChanged() || tab.numBoxes.hasValueChanged())
- {
- std::scoped_lock lock(propertiesMutex);
- properties.boxLayerName = tab.boxLayerName.getValue();
- properties.numBoxes = tab.numBoxes.getValue();
-
- {
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- }
- if (tab.drawBoxes.wasClicked())
- {
- // Lock shared variables in methods running in seperate threads
- // and pass them to functions. This way, the called functions do
- // not need to think about locking.
- std::scoped_lock lock(propertiesMutex, arvizMutex);
- drawBoxes(properties, arviz);
- }
- }
- */
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- void SickLaserUnit::drawBoxes(const SickLaserUnit::Properties& p, viz::Client&
- arviz)
- {
- // Draw something in ArViz (requires the armarx::ArVizComponentPluginUser.
- // See the ArVizExample in RobotAPI for more examples.
-
- viz::Layer layer = arviz.layer(p.boxLayerName);
- for (int i = 0; i < p.numBoxes; ++i)
+ std::string SickLaserUnit::getDefaultName() const
{
- layer.add(viz::Box("box_" + std::to_string(i))
- .position(Eigen::Vector3f(i * 100, 0, 0))
- .size(20).color(simox::Color::blue()));
- }
- arviz.commit(layer);
+ return "SickLaserUnit";
}
- */
} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index 69d6385c130969a4d18a6d104f91765ec4d9a876..f6ffc67a1ad3992967c8a4526dd6640b21caa4fb 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -54,30 +54,22 @@ namespace armarx
struct SickLaserScanDevice
{
- //scanner parameters
- std::string scannerType = "sick_tim_5xx";
- double angleOffset = 0.0;
- double rangeMin;
- double rangeMax;
- double timeIncrement;
//communication parameters
std::string ip;
std::string port;
int timelimit = 5;
- bool useTcp = false;
- char colaDialectId = 'A';
//data and task pointers
IceUtil::Time scanTime;
LaserScan scanData;
LaserScannerInfo scanInfo;
+ int initCnt = 0;
RunState runState = RunState::scannerFinalize;
RunningTask<SickLaserScanDevice>::pointer_type task;
std::string frameName = "LaserScannerFront";
LaserScannerUnitListenerPrx scanTopic;
-
- sick_scan::SickScanConfig cfg;
- sick_scan::SickGenericParser* parser;
- SickScanAdapter* scanner;
+ //scanner pointers
+ sick_scan::SickGenericParser* parser = 0;
+ SickScanAdapter* scanner = 0;
int result = sick_scan::ExitError;
bool isSensorInitialized = false;
@@ -134,12 +126,10 @@ namespace armarx
std::string topicName = "SICKLaserScanner";
//scanner parameters
std::string devices = "LaserScannerFront,192.168.8.133,2112";
- int timelimit = 5;
std::string scannerType = "sick_tim_5xx";
- double angleOffset = 0.0;
+ int timelimit = 5;
double rangeMin = 0.0;
double rangeMax = 10.0;
- double timeIncrement = 0.1;
};
Properties properties;
std::vector<SickLaserScanDevice> scanDevices;
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 1e9a011f3b044f6dbab583c5769d8c142fcf9a7c..69d33ced49a00c415f7e08456d21177c27dfb8b6 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -111,7 +111,6 @@ namespace armarx
port_(port),
timelimit_(timelimit)
{
-
if ((cola_dialect_id == 'a') || (cola_dialect_id == 'A'))
{
this->setProtocolType(CoLa_A);
@@ -155,9 +154,10 @@ namespace armarx
unsigned char receiveBuffer[65536];
int actual_length = 0;
int packetsInLoop = 0;
+ bool useBinaryProtocol = parser_ptr->getCurrentParamPtr()->getUseBinaryProtocol();
ros::Time recvTimeStamp = ros::Time::now(); // timestamp incoming package, will be overwritten by get_datagram
- int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, false, &packetsInLoop);
+ int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, useBinaryProtocol, &packetsInLoop);
//ros::Duration dur = recvTimeStampPush - recvTimeStamp;
if (result != 0)
{
@@ -347,6 +347,7 @@ namespace armarx
// 23: Starting angle (FFF92230)
if (updateScannerInfo)
{
+ scanInfo.device = hostname_;
uint starting_angle = (uint) - 1;
sscanf(fields[23], "%x", &starting_angle);
scanInfo.minAngle = (starting_angle / 10000.0) / 180.0 * M_PI - M_PI / 2;
@@ -438,7 +439,6 @@ namespace armarx
ARMARX_ERROR_S << "Number of distance measurements does not match number of intensity values - Skipping";
return sick_scan::ExitError;
}
- //TODO: Write ScanSteps with intensity
scanData.reserve(distVal.size());
for (int i = 0; i < (int) distVal.size(); i++)
{
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index a1ba8214cc56505b4a22af4d136d11e4cff5e7ac..56a17ef1928de4779241c59a432f7b6baaf50019 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -362,7 +362,7 @@ module armarx
void enableForceStop();
void disableForceStop();
- void setForceThreshold(float forceThreshold);
+ void setForceThreshold(Eigen::Vector3f forceThreshold);
};
class NJointTaskSpaceAdaptiveDMPControllerConfig extends NJointControllerConfig
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/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index 5fc97fcf5e21a6368abf52956457a7ec9f642193..d550b478c148c97372098e8e40dd7927bafa144f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -46,7 +46,8 @@ namespace armarx
forceOffset.setZero();
filteredForce.setZero();
filteredForceInRoot.setZero();
-
+ forceThreshold.getWriteBuffer() = cfg->forceThreshold;
+ forceThreshold.commitWrite();
tcp = rns->getTCP();
ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
ik->setDampedSvdLambda(0.0001);
@@ -292,7 +293,7 @@ namespace armarx
for (size_t i = 0; i < 3; ++i)
{
- if (fabs(filteredForceInRoot[i]) > cfg->forceThreshold[i])
+ if (fabs(filteredForceInRoot[i]) > forceThreshold.getUpToDateReadBuffer()[i])
{
stopPose = currentPose;
targetVel.setZero(6);
@@ -551,6 +552,11 @@ namespace armarx
usleep(100);
}
+ while (!interfaceData.updateReadBuffer())
+ {
+ usleep(100);
+ }
+
Eigen::Matrix4f pose = interfaceData.getUpToDateReadBuffer().currentTcpPose;
dmpCtrl->prepareExecution(dmpCtrl->eigen4f2vec(pose), goals);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index c0f8fea9525c5f261141a0658bdca7852a9eb263..3a6f685e412940d03b483a90dd92b9b346d9436f 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -94,9 +94,11 @@ namespace armarx
{
useForceStop = false;
}
- void setForceThreshold(float forceThreshold, const Ice::Current&) override
+
+ void setForceThreshold(const Eigen::Vector3f& f, const Ice::Current& current) override
{
- this->forceThreshold = forceThreshold;
+ forceThreshold.getWriteBuffer() = f;
+ forceThreshold.commitWrite();
}
protected:
virtual void onPublish(const SensorAndControl&, const DebugDrawerInterfacePrx&, const DebugObserverInterfacePrx&) override;
@@ -248,7 +250,7 @@ namespace armarx
Eigen::Vector3f filteredForce;
Eigen::Vector3f forceOffset;
Eigen::Vector3f filteredForceInRoot;
- std::atomic<float> forceThreshold;
+ WriteBufferedTripleBuffer<Eigen::Vector3f> forceThreshold;
std::atomic<bool> useForceStop;
std::atomic<float> timeForCalibration;
const SensorValueForceTorque* forceSensor;
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..354eec9cea9d1d1e37f4dc4ea790b47ba1749755 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)
@@ -235,24 +236,43 @@ namespace armarx::armem::server
// Core segment processors will aquire the core segment locks.
armem::wm::query_proc::MemoryQueryProcessor wmProcessor(
input.withData ? armem::DataMode::WithData : armem::DataMode::NoData);
- wm::Memory wmResult = wmProcessor.process(input, *workingMemory, /* execute if: */ { query::data::QueryTarget::WM });
+ wm::Memory wmResult = wmProcessor.process(input, *workingMemory);
armem::ltm::query_proc::MemoryQueryProcessor ltmProcessor;
- ltm::Memory ltmResult = ltmProcessor.process(input, *longtermMemory, /* execute if: */ { query::data::QueryTarget::LTM });
+ ltm::Memory ltmResult = ltmProcessor.process(input, *longtermMemory);
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);
+ 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_VERBOSE << "The LTM did not return data after query";
result.memory = toIce<data::MemoryPtr>(wmResult);
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
index 95943dd2f18289360336c22b910913835a84c604..2af92450309d640122bffc9066c1a1df7bdfff49 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/BaseQueryProcessorBase.h
@@ -22,34 +22,29 @@ namespace armarx::armem::base::query_proc
virtual ~BaseQueryProcessorBase() = default;
- DataT process(const QueryT& query, const DataT& data, const query::data::QueryTargets& executeIf = {}) const
+ DataT process(const QueryT& query, const DataT& data) const
{
DataT result = data.copyEmpty();
-
- for (const auto queryTarget : query.targets)
+ if (getTargets(query.targets).count(getTargetType()))
{
- if (std::find(executeIf.begin(), executeIf.end(), queryTarget) != executeIf.end())
- {
- this->process(result, query, data);
- break;
- }
+ this->process(result, query, data);
}
return result;
}
- DataT process(const QueryPtrT& query, const DataT& data, const query::data::QueryTargets& executeIf = {}) const
+ DataT process(const QueryPtrT& query, const DataT& data) const
{
- return this->process(*query, *data, executeIf);
+ return this->process(*query, *data);
}
- DataT process(const QuerySeqT& queries, const DataT& data, const query::data::QueryTargets& executeIf = {}) const
+ DataT process(const QuerySeqT& queries, const DataT& data) const
{
DataT result = data.copyEmpty();
- this->process(result, queries, data, executeIf);
+ this->process(result, queries, data);
return result;
}
- void process(DataT& result, const QuerySeqT& queries, const DataT& data, const query::data::QueryTargets& executeIf = {}) const
+ void process(DataT& result, const QuerySeqT& queries, const DataT& data) const
{
if (queries.empty())
{
@@ -57,35 +52,37 @@ namespace armarx::armem::base::query_proc
return;
}
- if (executeIf.empty())
- {
- ARMARX_DEBUG << "Could not execute query. ExecuteIf s empty.";
- return;
- }
-
for (const auto& query : queries)
{
- if (query->targets.empty())
+ if (getTargets(query->targets).count(getTargetType()))
{
- ARMARX_DEBUG << "The targets of a query are empty";
- continue;
- }
-
- for (const auto queryTarget : query->targets)
- {
- if (std::find(executeIf.begin(), executeIf.end(), queryTarget) != executeIf.end())
- {
- this->process(result, *query, data);
- break;
- }
- else
- {
- ARMARX_DEBUG << "The query target " << queryTarget << " was not found in executeIf: " << executeIf;
- }
+ this->process(result, *query, data);
}
}
}
virtual void process(DataT& result, const QueryT& query, const DataT& data) const = 0;
+
+
+ protected:
+
+ virtual query::data::QueryTarget getTargetType() const = 0;
+
+
+ private:
+
+ /// If empty, e.g. when receiving queries from python, we use WM as default.
+ /// We do it here as (Sl)ice does not support default values for vectors.
+ static std::set<query::data::QueryTarget> getTargets(const std::vector<query::data::QueryTarget>& _targets)
+ {
+ std::set<query::data::QueryTarget> targets(_targets.begin(), _targets.end());
+ if (targets.empty())
+ {
+ ARMARX_DEBUG << "Query has no targets - using WM as default.";
+ targets.insert(query::data::QueryTarget::WM);
+ }
+ return targets;
+ }
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
index cc06defc93976ea018ab8a7d3823a00d6121543d..55c5e51126b49775db7317d6401aa695c4b5581c 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/base/MemoryQueryProcessorBase.h
@@ -34,9 +34,9 @@ namespace armarx::armem::base::query_proc
using Base::process;
- _MemoryT process(const armem::query::data::Input& input, const _MemoryT& memory, const std::vector<query::data::QueryTarget>& executeIf = {}) const
+ _MemoryT process(const armem::query::data::Input& input, const _MemoryT& memory) const
{
- return this->process(input.memoryQueries, memory, executeIf);
+ return this->process(input.memoryQueries, memory);
}
void process(_MemoryT& result,
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.h
index 84f967795fd26d79d173a14e71498dbec7d56ee1..469b5f907d01bbff757b1bea14ff8e80e5769315 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.h
@@ -22,5 +22,11 @@ namespace armarx::armem::d_ltm::query_proc
{}
protected:
+
+ query::data::QueryTarget getTargetType() const override
+ {
+ return query::data::QueryTarget::Disk;
+ }
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/CoreSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/CoreSegmentQueryProcessor.h
index 1c40f7fc64b0a2c59fdc3e54d968c6627480cbb0..5c043a2d1dbe2218078591adc3fc0ebac0bfe932 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/CoreSegmentQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/CoreSegmentQueryProcessor.h
@@ -27,7 +27,7 @@ namespace armarx::armem::d_ltm::query_proc
protected:
virtual ProviderSegmentT providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegmentT& s) const override
{
- return providerSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::Disk});
+ return providerSegmentProcessor.process(q, s);
}
private:
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.h
index 2abdcc1ccc7c25a317edc67e36835cb4a4df2f9b..038523950bfe399c2ef7a65b79fde43376651617 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/MemoryQueryProcessor.h
@@ -26,7 +26,7 @@ namespace armarx::armem::d_ltm::query_proc
protected:
virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
{
- return coreSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::Disk});
+ return coreSegmentProcessor.process(q, s);
}
private:
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/ProviderSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/ProviderSegmentQueryProcessor.h
index 94c2e756875c1152f8018f70c5e0d691a5704582..2043273ec7c5f3fee2d02410a145ef436c47459c 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/ProviderSegmentQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/ProviderSegmentQueryProcessor.h
@@ -26,7 +26,7 @@ namespace armarx::armem::d_ltm::query_proc
protected:
virtual EntityT entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const EntityT& s) const override
{
- return entityProcessor.process(q, s, {armem::query::data::QueryTarget::Disk});
+ return entityProcessor.process(q, s);
}
private:
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h
index 458128e5305139db33a6c25d6ff19e3e92772fd5..64f560afff93e5d0cfff4b036f68f0fb4e8cdaba 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/BaseQueryProcessor.h
@@ -22,5 +22,11 @@ namespace armarx::armem::ltm::query_proc
{}
protected:
+
+ query::data::QueryTarget getTargetType() const override
+ {
+ return query::data::QueryTarget::LTM;
+ }
+
};
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h
index e333dd816aedabbb8dfa7209c687caf727d9ccf9..dc379f1d88fa82bbe886e412e0341079fa43fce9 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/CoreSegmentQueryProcessor.h
@@ -27,7 +27,7 @@ namespace armarx::armem::ltm::query_proc
protected:
virtual ProviderSegmentT providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegmentT& s) const override
{
- return providerSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::LTM});
+ return providerSegmentProcessor.process(q, s);
}
private:
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.h
index 569014d6579eeb6c99996e4be5b7cb68f65457b1..a5129417262515975efa247398602272865ee5ec 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/MemoryQueryProcessor.h
@@ -26,7 +26,7 @@ namespace armarx::armem::ltm::query_proc
protected:
virtual CoreSegmentT coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegmentT& s) const override
{
- return coreSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::LTM});
+ return coreSegmentProcessor.process(q, s);
}
private:
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h
index 5443acfd6594b98250711cf2acfd434a98414066..e0c6db9e3ef06e6241e9af8c572bb7fc78eec452 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/longtermmemory/ProviderSegmentQueryProcessor.h
@@ -26,7 +26,7 @@ namespace armarx::armem::ltm::query_proc
protected:
virtual EntityT entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const EntityT& s) const override
{
- return entityProcessor.process(q, s, {armem::query::data::QueryTarget::LTM});
+ return entityProcessor.process(q, s);
}
private:
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
index 933f9a6fd628b8c2747c2c6449499e596593935b..d4875053c93dc2278cdb9f327fc0739370811fe7 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/BaseQueryProcessor.h
@@ -26,6 +26,11 @@ namespace armarx::armem::wm::query_proc
protected:
+ query::data::QueryTarget getTargetType() const override
+ {
+ return query::data::QueryTarget::WM;
+ }
+
DataMode dataMode;
};
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
index bda9ad04de5ac7cd46be68a2f5f7ca2ec1419040..4c41f16664af134b66b00355080d8de362d7eaf9 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/CoreSegmentQueryProcessor.cpp
@@ -32,7 +32,7 @@ namespace armarx::armem::wm::query_proc
ProviderSegment CoreSegmentQueryProcessor::providerSegmentProcessorProcess(const armem::query::data::ProviderSegmentQuerySeq& q, const ProviderSegment& s) const
{
- return providerSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
+ return providerSegmentProcessor.process(q, s);
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
index 7ef31f2badfbffbf49f132c855b2143b8ddc10bd..005649b8f670586f16177f90a43c6e0f48b3879d 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/EntityQueryProcessor.cpp
@@ -6,7 +6,7 @@
namespace armarx::armem::wm::query_proc
{
EntityQueryProcessor::EntityQueryProcessor(DataMode dataMode) :
- Base(dataMode)
+ BaseQueryProcessor<wm::Entity, armem::query::data::EntityQuery>(dataMode)
{}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
index dd249b61caaacaca11e369b19b8f4e0da1dc5cf5..7aa868af13bc525b42d93f230a8398c27531525d 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/MemoryQueryProcessor.cpp
@@ -16,7 +16,7 @@ namespace armarx::armem::wm::query_proc
CoreSegment MemoryQueryProcessor::coreSegmentProcessorProcess(const armem::query::data::CoreSegmentQuerySeq& q, const CoreSegment& s) const
{
- return coreSegmentProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
+ return coreSegmentProcessor.process(q, s);
}
}
diff --git a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
index a5e23184fa999dc44d72de7588cf77fad0503729..eb2524631b4c1d2fb89eb3e7c36a3aaf86f31e29 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
+++ b/source/RobotAPI/libraries/armem/server/query_proc/workingmemory/ProviderSegmentQueryProcessor.cpp
@@ -24,7 +24,7 @@ namespace armarx::armem::wm::query_proc
Entity ProviderSegmentQueryProcessor::entityProcessorProcess(const armem::query::data::EntityQuerySeq& q, const Entity& s) const
{
- return entityProcessor.process(q, s, {armem::query::data::QueryTarget::WM});
+ return entityProcessor.process(q, s);
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index 5037bad396c0e7a83b7704b4895a25225ae8106f..f88a2e1731c87de6fa163c307c90f4c6d3e01ae2 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -245,7 +245,7 @@ namespace armarx::armem::gui
input.addQueryTargetToAll(armem::query::data::QueryTarget::Disk);
armem::d_ltm::query_proc::MemoryQueryProcessor d_ltm_processor;
- dMem = d_ltm_processor.process(input.toIce(), dMem, /* execute if: */ { query::data::QueryTarget::Disk });
+ dMem = d_ltm_processor.process(input.toIce(), dMem);
wm::Memory converted = dMem.convert();
memoryData[k] = std::move(converted);
@@ -275,7 +275,7 @@ namespace armarx::armem::gui
// requery (e.g. to get only the last n values instead of the last n from disk and the last n from wm)
armem::wm::query_proc::MemoryQueryProcessor wm_processor;
- result.memory = wm_processor.process(input.toIce(), result.memory, /* execute if: */ { query::data::QueryTarget::WM });
+ result.memory = wm_processor.process(input.toIce(), result.memory);
if (!result.memory.hasData())
{
@@ -293,7 +293,7 @@ namespace armarx::armem::gui
}
else
{
- ARMARX_WARNING << "A query for memory '" + name + "' produced an error: " << result.errorMessage;
+ ARMARX_WARNING << "A query for memory '" << name << "' produced an error: " << result.errorMessage;
if (statusLabel)
{
statusLabel->setText(QString::fromStdString(result.errorMessage));