diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
index 5f99047a649e80be511efd37c1cfbc8ac438031a..778c2db5fd47742488efb3ee0f8ce6aaae4fe0e3 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
@@ -5,6 +5,7 @@
#include <Inventor/sensors/SoTimerSensor.h>
#include <Inventor/nodes/SoUnits.h>
+#include <QCoreApplication>
#include <thread>
@@ -88,11 +89,6 @@ namespace armarx::viz
&CoinVisualizerWrapper::onUpdateSuccess,
&CoinVisualizerWrapper::onUpdateFailure);
root = new SoSeparator;
-
- //timerSensor = new SoTimerSensor(updateVisualizationCB, this);
-
- //float cycleTimeMS = 33.0f;
- //timerSensor->setInterval(SbTime(cycleTimeMS / 1000.0f));
}
CoinVisualizer::~CoinVisualizer()
@@ -114,9 +110,6 @@ namespace armarx::viz
}
state = CoinVisualizerState::STARTING;
stateStorage = storage;
-
- //SoSensorManager* sensor_mgr = SoDB::getSensorManager();
- //sensor_mgr->insertTimerSensor(timerSensor);
}
void CoinVisualizer::stop()
@@ -126,15 +119,12 @@ namespace armarx::viz
return;
}
- SoSensorManager* sensor_mgr = SoDB::getSensorManager();
state = CoinVisualizerState::STOPPING;
while (state != CoinVisualizerState::STOPPED)
{
- sensor_mgr->processTimerQueue();
+ QCoreApplication::processEvents();
usleep(1000);
}
- sensor_mgr->removeTimerSensor(timerSensor);
-
}
CoinVisualizer_ApplyTiming CoinVisualizer::apply(data::LayerUpdate const& update)
diff --git a/source/RobotAPI/components/ArViz/Coin/Visualizer.h b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
index 49ad2a51bd6f70dfddac1785296b8abc07ba08c9..1494be07dc5615b787f2a50f12a48451b40e045a 100644
--- a/source/RobotAPI/components/ArViz/Coin/Visualizer.h
+++ b/source/RobotAPI/components/ArViz/Coin/Visualizer.h
@@ -229,7 +229,6 @@ namespace armarx::viz
std::mutex storageMutex;
viz::StorageInterfacePrx storage;
- SoTimerSensor* timerSensor = nullptr;
CoinLayerMap layers;
std::vector<std::type_index> elementVisualizersTypes;
diff --git a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
index 15153b3dca0bac00d1f8a6192191cead86227a05..f1e94f9b2478222809011ad2c150f2b97d0c9e72 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
+++ b/source/RobotAPI/drivers/SickLaserUnit/CMakeLists.txt
@@ -33,7 +33,7 @@ armarx_add_component(
# RobotAPI
RobotAPICore
## RobotAPIInterfaces
- ## RobotAPIComponentPlugins # For ArViz and other plugins.
+ RobotAPIComponentPlugins # For ArViz and other plugins.
# This project
## ${PROJECT_NAME}Interfaces # For ice interfaces from this package.
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
index 5498ee1d8538633506bb7045128f5cf150ac1b55..04fa0b34bf676e53beae6ba6cdaf185598d72447 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.cpp
@@ -43,8 +43,6 @@ namespace armarx
{
initCnt++;
initScanner();
- //read the scanner parameters for initialization
- result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
}
else
{
@@ -59,10 +57,10 @@ namespace armarx
if (scanTopic)
{
TimestampVariantPtr scanT(new TimestampVariant(scanTime));
- scanTopic->reportSensorValues(ip, frameName, scanData, scanT);
+ scanTopic->reportSensorValues(ip, scannerName, scanData, scanT);
scanData.clear();
//trigger heartbeat
- //heartbeat.heartbeat(channel);
+ scannerHeartbeat->heartbeat(scannerName);
}
else
{
@@ -87,25 +85,25 @@ namespace armarx
void SickLaserScanDevice::initScanner()
{
- this->isSensorInitialized = false;
ARMARX_INFO_S << "Start initialising scanner [Ip: " << this->ip
<< "] [Port: " << this->port << "]";
// attempt to connect/reconnect
if (this->scanner)
{
ARMARX_WARNING_S << "Scanner already initized.";
- delete this->scanner; // disconnect scanner
+ this->scanner.reset(); // disconnect scanner
}
-
- this->scanner = new SickScanAdapter(this->ip, this->port, this->timelimit, this->parser, 'A');
-
+ this->scanner = std::make_unique<SickScanAdapter>(this->ip, this->port, this->timelimit, this->parser.get(), 'A');
this->result = this->scanner->init();
-
+ if (this->result == sick_scan::ExitSuccess)
+ {
+ //read the scanner parameters for initialization
+ this->result = scanner->loopOnce(scanData, scanTime, scanInfo, true);
+ }
if (this->result == sick_scan::ExitSuccess) // OK -> loop again
{
- this->isSensorInitialized = true;
- ARMARX_INFO_S << "Scanner initialized.";
+ ARMARX_INFO_S << "Scanner successfully initialized.";
this->runState = RunState::scannerRun; // after initialising switch to run state
}
else
@@ -123,9 +121,6 @@ namespace armarx
// Publish to a topic (passing the TopicListenerPrx).
// def->topic(myTopicListener);
- // Subscribe to a topic (passing the topic name).
- //def->topic<LaserScannerUnitListenerPrx>(properties.topicName);
-
// Use (and depend on) another component (passing the ComponentInterfacePrx).
// def->component(myComponentProxy)
@@ -139,12 +134,18 @@ namespace armarx
return def;
}
+ SickLaserUnit::SickLaserUnit()
+ {
+ //heartbeat = addPlugin<plugins::HeartbeatComponentPlugin>("SickLaserUnit");
+ addPlugin(heartbeat);
+ ARMARX_CHECK_NOT_NULL(heartbeat);
+ }
+
void SickLaserUnit::onInitComponent()
{
+ ARMARX_INFO << "On init";
// 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);
@@ -163,8 +164,7 @@ namespace armarx
}
SickLaserScanDevice& device = scanDevices.emplace_back();
device.scanTopic = topic;
- device.isSensorInitialized = false;
- device.frameName = deviceInfo[0];
+ device.scannerName = deviceInfo[0];
if (deviceInfo[1] != "")
{
device.ip = deviceInfo[1];
@@ -179,11 +179,11 @@ namespace armarx
device.timelimit = properties.timelimit;
//scanInfo
device.scanInfo.device = device.ip;
- device.scanInfo.frame = device.frameName;
+ device.scanInfo.frame = device.scannerName;
//scanner Parameters
try
{
- device.parser = new sick_scan::SickGenericParser(properties.scannerType);
+ device.parser = std::make_unique<sick_scan::SickGenericParser>(properties.scannerType);
device.parser->set_range_min(properties.rangeMin);
device.parser->set_range_max(properties.rangeMax);
device.parser->getCurrentParamPtr()->setUseBinaryProtocol(false);
@@ -193,16 +193,13 @@ namespace armarx
ARMARX_ERROR_S << "Could not create parser. Wrong Scanner name.";
return;
}
-
- //addPlugin(heartbeat);
- //configureHeartbeatChannel();
+ device.scannerHeartbeat = heartbeat;
+ device.scannerHeartbeat->configureHeartbeatChannel(device.scannerName, armarx::RobotHealthHeartbeatArgs(100, 200, "No updates available"));
}
}
void SickLaserUnit::onConnectComponent()
{
- //topic = getTopic<LaserScannerUnitListenerPrx>(properties.topicName);
-
for (SickLaserScanDevice& device : scanDevices)
{
device.scanTopic = topic;
@@ -246,20 +243,6 @@ namespace armarx
void SickLaserUnit::onExitComponent()
{
- for (SickLaserScanDevice& device : scanDevices)
- {
-
- if (device.scanner)
- {
- delete device.scanner;
- }
- if (device.parser)
- {
- delete device.parser;
- }
- }
-
-
}
std::string SickLaserUnit::getDefaultName() const
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
index f6ffc67a1ad3992967c8a4526dd6640b21caa4fb..662260de57aea4188e45bc6052061a9d01d19598 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickLaserUnit.h
@@ -27,7 +27,8 @@
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/services/tasks/RunningTask.h>
#include <RobotAPI/interface/units/LaserScannerUnit.h>
-// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
+//#include <RobotAPI/interface/components/RobotHealthInterface.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/HeartbeatComponentPlugin.h>
#include <vector>
@@ -54,6 +55,7 @@ namespace armarx
struct SickLaserScanDevice
{
+ std::string scannerName = "LaserScannerFront";
//communication parameters
std::string ip;
std::string port;
@@ -63,15 +65,14 @@ namespace armarx
LaserScan scanData;
LaserScannerInfo scanInfo;
int initCnt = 0;
+ int result = sick_scan::ExitError;
RunState runState = RunState::scannerFinalize;
RunningTask<SickLaserScanDevice>::pointer_type task;
- std::string frameName = "LaserScannerFront";
LaserScannerUnitListenerPrx scanTopic;
+ plugins::HeartbeatComponentPlugin* scannerHeartbeat;
//scanner pointers
- sick_scan::SickGenericParser* parser = 0;
- SickScanAdapter* scanner = 0;
- int result = sick_scan::ExitError;
- bool isSensorInitialized = false;
+ std::unique_ptr<sick_scan::SickGenericParser> parser;
+ std::unique_ptr<SickScanAdapter> scanner;
void initScanner();
void run();
@@ -90,13 +91,14 @@ namespace armarx
class SickLaserUnit :
//virtual public armarx::LaserScannerUnitInterface,
virtual public armarx::Component
- // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::RobotHealthComponentUser
// , virtual public armarx::LightweightRemoteGuiComponentPluginUser
// , virtual public armarx::ArVizComponentPluginUser
{
public:
/// @see armarx::ManagedIceObject::getDefaultName()
std::string getDefaultName() const override;
+ SickLaserUnit();
protected:
/// @see PropertyUser::createPropertyDefinitions()
@@ -134,29 +136,7 @@ namespace armarx
Properties properties;
std::vector<SickLaserScanDevice> scanDevices;
LaserScannerUnitListenerPrx topic;
- //HeartbeatComponentPlugin heartbeat;
-
- /* Use a mutex if you access variables from different threads
- * (e.g. ice functions and RemoteGui_update()).
- std::mutex propertiesMutex;
- */
+ plugins::HeartbeatComponentPlugin* heartbeat;
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- /// Tab shown in the Remote GUI.
- struct RemoteGuiTab : armarx::RemoteGui::Client::Tab
- {
- armarx::RemoteGui::Client::LineEdit boxLayerName;
- armarx::RemoteGui::Client::IntSpinBox numBoxes;
-
- armarx::RemoteGui::Client::Button drawBoxes;
- };
- RemoteGuiTab tab;
- */
-
- /* (Requires the armarx::ArVizComponentPluginUser.)
- * When used from different threads, an ArViz client needs to be synchronized.
- /// Protects the arviz client inherited from the ArViz plugin.
- std::mutex arvizMutex;
- */
};
} // namespace armarx
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
index 69d33ced49a00c415f7e08456d21177c27dfb8b6..715265cb484cabb48629d08b770ece7171a567d6 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.cpp
@@ -84,8 +84,8 @@
#define deg2rad_const (0.017453292519943295769236907684886f)
-std::vector<unsigned char> exampleData(65536);
-std::vector<unsigned char> receivedData(65536);
+//std::vector<unsigned char> exampleData(65536);
+//std::vector<unsigned char> receivedData(65536);
//static long receivedDataLen = 0;
static int getDiagnosticErrorCode()
@@ -106,7 +106,6 @@ namespace armarx
sick_scan::SickScanCommon(parser),
socket_(io_service_),
deadline_(io_service_),
- parser_ptr(parser),
hostname_(hostname),
port_(port),
timelimit_(timelimit)
@@ -120,7 +119,7 @@ namespace armarx
{
this->setProtocolType(CoLa_B);
}
- assert(this->getProtocolType() != CoLa_Unknown);
+ ARMARX_CHECK(this->getProtocolType() != CoLa_Unknown);
m_numberOfBytesInReceiveBuffer = 0;
m_alreadyReceivedBytes = 0;
@@ -136,7 +135,7 @@ namespace armarx
SickScanAdapter::~SickScanAdapter()
{
- // stop_scanner();
+ //stopScanData();
close_device();
}
@@ -146,7 +145,7 @@ namespace armarx
/*!
- \brief parsing datagram and publishing ros messages
+ \brief parsing datagram into ARMARX message
\return error code
*/
int SickScanAdapter::loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo)
@@ -154,10 +153,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, useBinaryProtocol, &packetsInLoop);
+ //use ASCII always
+ int result = get_datagram(recvTimeStamp, receiveBuffer, 65536, &actual_length, false, &packetsInLoop);
//ros::Duration dur = recvTimeStampPush - recvTimeStamp;
if (result != 0)
{
@@ -169,12 +168,10 @@ namespace armarx
return sick_scan::ExitSuccess;
} // return success to continue looping
- //TODO: convert ros::Time recvTimeStamp to IceUtil
+ //convert ros::Time recvTimeStamp to IceUtil
ros::Time correctedStamp = recvTimeStamp + ros::Duration(config_.time_offset);
uint64_t recvMsec = correctedStamp.toNSec() / 1000;
- scanTime = IceUtil::Time::milliSeconds(recvMsec);
- //scanTime = TimeUtil::GetTime();
- //msg.header.stamp = recvTimeStamp + ros::Duration(config_.time_offset);
+ scanTime = IceUtil::Time::microSeconds(recvMsec);
//datagrams are enclosed in <STX> (0x02), <ETX> (0x03) pairs
char* buffer_pos = (char*) receiveBuffer;
@@ -461,7 +458,6 @@ namespace armarx
void SickScanAdapter::disconnectFunction()
{
-
}
void SickScanAdapter::disconnectFunctionS(void* obj)
diff --git a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
index 5b7e772f33217ed39e9c708275c6f7e5d10b2251..1707952d9583de18bd6de004f986e216e9c13781 100644
--- a/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
+++ b/source/RobotAPI/drivers/SickLaserUnit/SickScanAdapter.h
@@ -72,10 +72,6 @@ namespace armarx
class SickScanAdapter : public sick_scan::SickScanCommon
{
public:
- int loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
-
- int parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
-
static void disconnectFunctionS(void* obj);
SickScanAdapter(const std::string& hostname, const std::string& port, int& timelimit, sick_scan::SickGenericParser* parser,
@@ -83,6 +79,10 @@ namespace armarx
virtual ~SickScanAdapter();
+ int loopOnce(LaserScan& scanData, IceUtil::Time& scanTime, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
+
+ int parseDatagram(char* datagram, size_t datagram_length, LaserScan& scanData, LaserScannerInfo& scanInfo, bool updateScannerInfo = false);
+
static void readCallbackFunctionS(void* obj, UINT8* buffer, UINT32& numOfBytes);
void readCallbackFunction(UINT8* buffer, UINT32& numOfBytes);
@@ -163,7 +163,6 @@ namespace armarx
boost::system::error_code ec_;
size_t bytes_transfered_;
- sick_scan::SickGenericParser* parser_ptr;
std::string hostname_;
std::string port_;
int timelimit_;
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
index 4f7372a5e178ed8ec895049ef094af354d297ee2..e210b778f25dd9ce8f83bfd299ff3ec4c75497e2 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.cpp
@@ -810,30 +810,34 @@ namespace armarx
void ArVizWidgetController::onReplayTimerTick()
{
- if (mode != ArVizWidgetMode::ReplayingTimed)
+ if (mode == ArVizWidgetMode::ReplayingTimed)
{
- return;
- }
- double replaySpeed = widget.replaySpeedSpinBox->value();
+ double replaySpeed = widget.replaySpeedSpinBox->value();
- replayCurrentTimestamp += 33000 * replaySpeed;
+ long currentRealTime = IceUtil::Time::now().toMicroSeconds();
+ long elapsedRealTime = currentRealTime - lastReplayRealTime;
- long revision = getRevisionForTimestamp(replayCurrentTimestamp);
- if (revision == -2)
- {
- if (widget.replayLoopbackCheckBox->checkState() == Qt::Checked)
+ replayCurrentTimestamp += elapsedRealTime * replaySpeed;
+
+ long revision = getRevisionForTimestamp(replayCurrentTimestamp);
+ if (revision == -2)
{
- replayCurrentTimestamp = currentRecording.firstTimestampInMicroSeconds;
+ if (widget.replayLoopbackCheckBox->checkState() == Qt::Checked)
+ {
+ replayCurrentTimestamp = currentRecording.firstTimestampInMicroSeconds;
+ }
+ else
+ {
+ revision = currentRecording.lastRevision;
+ }
}
- else
+ if (revision >= 0)
{
- revision = currentRecording.lastRevision;
+ widget.replayRevisionSlider->setValue(revision);
}
}
- if (revision >= 0)
- {
- widget.replayRevisionSlider->setValue(revision);
- }
+
+ lastReplayRealTime = IceUtil::Time::now().toMicroSeconds();
}
void ArVizWidgetController::changeMode(ArVizWidgetMode newMode)
@@ -917,10 +921,16 @@ namespace armarx
auto& entry = recordingBatchCache[batch.header.index];
entry.data = batch;
entry.lastUsed = IceUtil::Time::now();
+
+ limitRecordingBatchCacheSize();
+
+ recordingWaitingForBatchIndex = -1;
}
viz::data::RecordingBatch const& ArVizWidgetController::getRecordingBatch(long index)
{
+ ARMARX_TRACE;
+
IceUtil::Time now = IceUtil::Time::now();
std::unique_lock<std::mutex> lock(recordingBatchCacheMutex);
@@ -930,16 +940,23 @@ namespace armarx
{
// Start prefetching neighbouring batches
bool asyncPrefetchIsRunning = callbackResult && !callbackResult->isCompleted();
- if (!asyncPrefetchIsRunning)
+ if (!asyncPrefetchIsRunning && recordingWaitingForBatchIndex == -1)
{
if (index + 1 < long(currentRecording.batchHeaders.size())
&& recordingBatchCache.count(index + 1) == 0)
{
+ // ARMARX_WARNING << "after begin_getRecordingBatch: " << (index + 1)
+ // << " waiting for " << recordingWaitingForBatchIndex;
callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index + 1, callback);
+ recordingWaitingForBatchIndex = index + 1;
+ ARMARX_INFO << "Now waiting for " << recordingWaitingForBatchIndex;
}
else if (index > 0 && recordingBatchCache.count(index - 1) == 0)
{
+ // ARMARX_WARNING << "before begin_getRecordingBatch: " << (index - 1)
+ // << " waiting for " << recordingWaitingForBatchIndex;
callbackResult = storage->begin_getRecordingBatch(currentRecording.id, index - 1, callback);
+ recordingWaitingForBatchIndex = index - 1;
}
}
@@ -949,6 +966,19 @@ namespace armarx
return entry.data;
}
+ // Maybe there has already been sent a asynchronous request to get
+ if (index == recordingWaitingForBatchIndex)
+ {
+ lock.unlock();
+ ARMARX_INFO << "Waiting to receive async batch: " << index;
+ // Wait until request completes
+ while (recordingWaitingForBatchIndex != -1)
+ {
+ QCoreApplication::processEvents();
+ }
+ return getRecordingBatch(index);
+ }
+
ARMARX_INFO << "Batch #" << index << " is not in the cache. Getting synchronously, blocking GUI...";
// Entry is not in the cache, we have to get it from ArVizStorage
@@ -956,6 +986,13 @@ namespace armarx
newEntry.lastUsed = now;
newEntry.data = storage->getRecordingBatch(currentRecording.id, index);
+ limitRecordingBatchCacheSize();
+
+ return newEntry.data;
+ }
+
+ void ArVizWidgetController::limitRecordingBatchCacheSize()
+ {
if (recordingBatchCache.size() > recordingBatchCacheMaxSize)
{
// Remove the entry with the oldest last used timestamp
@@ -972,8 +1009,6 @@ namespace armarx
recordingBatchCache.erase(oldestIter);
}
-
- return newEntry.data;
}
SoNode* ArVizWidgetController::getScene()
diff --git a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
index 31383a63ea78c510ec781951ff56b5fdbcafee06..4a493dbd19a4c4cab6e05aa2a7ab5376936a3ebe 100644
--- a/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
+++ b/source/RobotAPI/gui-plugins/ArViz/ArVizWidgetController.h
@@ -182,6 +182,7 @@ namespace armarx
QTimer* replayTimer;
long replayCurrentTimestamp = 0;
+ long lastReplayRealTime = 0;
std::string storageName;
armarx::viz::StorageInterfacePrx storage;
@@ -207,9 +208,11 @@ namespace armarx
};
viz::data::RecordingBatch const& getRecordingBatch(long index);
+ void limitRecordingBatchCacheSize();
std::size_t recordingBatchCacheMaxSize = 5;
std::mutex recordingBatchCacheMutex;
+ std::atomic<long> recordingWaitingForBatchIndex = -1;
std::map<long, TimestampedRecordingBatch> recordingBatchCache;
ArVizWidgetMode mode = ArVizWidgetMode::NotConnected;
diff --git a/source/RobotAPI/interface/armem/query.ice b/source/RobotAPI/interface/armem/query.ice
index 2a1586fece346e89cbe491cd21c026ddab603e0e..936294507f0b05627051fa6974309faa1db4d758 100644
--- a/source/RobotAPI/interface/armem/query.ice
+++ b/source/RobotAPI/interface/armem/query.ice
@@ -14,8 +14,7 @@ module armarx
enum QueryTarget
{
WM,
- LTM,
- Disk
+ LTM
};
sequence<QueryTarget> QueryTargets;
diff --git a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
index 56a17ef1928de4779241c59a432f7b6baaf50019..84614f0d5d98bb23fc63a14bc4c0a237189b2779 100644
--- a/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
+++ b/source/RobotAPI/interface/units/RobotUnit/NJointTaskSpaceDMPController.ice
@@ -336,6 +336,7 @@ module armarx
void setUseNullSpaceJointDMP(bool enable);
bool isFinished();
+ bool isDMPRunning();
void runDMP(Ice::DoubleSeq goals);
void runDMPWithTime(Ice::DoubleSeq goals, double timeDuration);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
index 79a090adbae49e70b400ca3cd358b0599a11c9f1..e8a401f485b38a2c85f4a9a718f94d8a1d0a905a 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.cpp
@@ -472,6 +472,7 @@ namespace armarx
debugRT.getWriteBuffer().targetVel = targetVel;
debugRT.getWriteBuffer().adaptK = adaptK;
debugRT.getWriteBuffer().isPhaseStop = isPhaseStop;
+ debugRT.getWriteBuffer().currentTwist = currentTwist;
rt2CtrlData.getWriteBuffer().currentPose = currentPose;
rt2CtrlData.getWriteBuffer().currentTwist = currentTwist;
@@ -673,6 +674,11 @@ namespace armarx
datafields["targetVel_y"] = new Variant(targetVel(1));
datafields["targetVel_z"] = new Variant(targetVel(2));
+ Eigen::VectorXf currentVel = debugRT.getUpToDateReadBuffer().currentTwist;
+ datafields["currentVel_x"] = new Variant(currentVel(0));
+ datafields["currentVel_y"] = new Variant(currentVel(1));
+ datafields["currentVel_z"] = new Variant(currentVel(2));
+
Eigen::Vector3f adaptK = debugRT.getUpToDateReadBuffer().adaptK;
datafields["adaptK_x"] = new Variant(adaptK(0));
datafields["adaptK_y"] = new Variant(adaptK(1));
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
index ebb3b05a5241c338cbdfc73ebd30ef3226d7219d..477feb6900efcdaca0981deb92534cc5dfb0c172 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointPeriodicTSDMPCompliantController.h
@@ -99,6 +99,7 @@ namespace armarx
Eigen::Vector3f adaptK;
Eigen::VectorXf targetVel;
Eigen::Matrix4f currentPose;
+ Eigen::VectorXf currentTwist;
bool isPhaseStop;
float manidist;
};
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
index d550b478c148c97372098e8e40dd7927bafa144f..0a08c5396283856d5e5ccc5e8ebfdbb9cb59500c 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.cpp
@@ -11,12 +11,14 @@ namespace armarx
const armarx::NJointControllerConfigPtr& config,
const VirtualRobot::RobotPtr&)
{
+ ARMARX_TRACE;
ARMARX_INFO << "creating impedance dmp controller";
cfg = NJointTaskSpaceImpedanceDMPControllerConfigPtr::dynamicCast(config);
+ ARMARX_CHECK_NOT_NULL(cfg);
useSynchronizedRtRobot();
rns = rtGetRobot()->getRobotNodeSet(cfg->nodeSetName);
ARMARX_CHECK_EXPRESSION(rns) << cfg->nodeSetName;
-
+ ARMARX_INFO << "1";
for (size_t i = 0; i < rns->getSize(); ++i)
{
std::string jointName = rns->getNode(i)->getName();
@@ -39,19 +41,20 @@ namespace armarx
velocitySensors.push_back(velocitySensor);
positionSensors.push_back(positionSensor);
};
-
const SensorValueBase* svlf = robotUnit->getSensorDevice(cfg->forceSensorName)->getSensorValue();
forceSensor = svlf->asA<SensorValueForceTorque>();
+ ARMARX_TRACE;
forceOffset.setZero();
filteredForce.setZero();
filteredForceInRoot.setZero();
- forceThreshold.getWriteBuffer() = cfg->forceThreshold;
- forceThreshold.commitWrite();
+ ARMARX_INFO << cfg->forceThreshold;
+ forceThreshold.reinitAllBuffers(cfg->forceThreshold);
tcp = rns->getTCP();
ik.reset(new VirtualRobot::DifferentialIK(rns, rtGetRobot()->getRootNode(), VirtualRobot::JacobiProvider::eSVDDamped));
ik->setDampedSvdLambda(0.0001);
+ ARMARX_TRACE;
numOfJoints = targets.size();
// set DMP
TaskSpaceDMPControllerConfig taskSpaceDMPConfig;
@@ -88,6 +91,7 @@ namespace armarx
}
defaultNullSpaceJointValues.reinitAllBuffers(nullspaceValues);
+ ARMARX_TRACE;
Eigen::Vector3f kpos(cfg->Kpos[0], cfg->Kpos[1], cfg->Kpos[2]);
Eigen::Vector3f dpos(cfg->Dpos[0], cfg->Dpos[1], cfg->Dpos[2]);
Eigen::Vector3f kori(cfg->Kori[0], cfg->Kori[1], cfg->Kori[2]);
@@ -136,6 +140,7 @@ namespace armarx
void NJointTaskSpaceImpedanceDMPController::rtPreActivateController()
{
+ ARMARX_TRACE;
NJointTaskSpaceImpedanceDMPControllerControlData initData;
initData.targetPose = tcp->getPoseInRootFrame();
initData.targetVel.resize(6);
@@ -547,6 +552,8 @@ namespace armarx
{
firstRun = true;
timeForCalibration = 0;
+ started = false;
+
while (firstRun || timeForCalibration < cfg->waitTimeForCalibration)
{
usleep(100);
diff --git a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
index 3a6f685e412940d03b483a90dd92b9b346d9436f..58fd840fb1121b4a159c185abb2e72fd7470f082 100644
--- a/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
+++ b/source/RobotAPI/libraries/RobotAPINJointControllers/DMPController/NJointTaskSpaceImpedanceDMPController.h
@@ -56,6 +56,11 @@ namespace armarx
return finished;
}
+ bool isDMPRunning(const Ice::Current&) override
+ {
+ return started;
+ }
+
void setViaPoints(Ice::Double u, const Ice::DoubleSeq& viapoint, const Ice::Current&) override;
void setGoals(const Ice::DoubleSeq& goals, const Ice::Current&) override;
diff --git a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
index 2080879c55f1dd94e5aeeb2f6d78f72ada5c4aaf..31636867698321b0d04a52d4c8946d82d7b78eb8 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.cpp
@@ -13,7 +13,6 @@ namespace armarx::armem::ltm
Memory::Memory(const Memory& other) :
Base(other),
dbsettings(other.dbsettings),
- alwaysTransferSettings(other.alwaysTransferSettings),
periodicTransferSettings(other.periodicTransferSettings),
onFullTransferSettings(other.onFullTransferSettings),
mongoDBMutex()
@@ -25,7 +24,6 @@ namespace armarx::armem::ltm
Memory::Memory(Memory&& other) :
Base(other),
dbsettings(other.dbsettings),
- alwaysTransferSettings(other.alwaysTransferSettings),
periodicTransferSettings(other.periodicTransferSettings),
onFullTransferSettings(other.onFullTransferSettings),
reloaded(other.reloaded)
@@ -38,7 +36,6 @@ namespace armarx::armem::ltm
{
Base::operator=(other);
dbsettings = other.dbsettings;
- alwaysTransferSettings = other.alwaysTransferSettings;
periodicTransferSettings = other.periodicTransferSettings;
onFullTransferSettings = other.onFullTransferSettings;
@@ -51,7 +48,6 @@ namespace armarx::armem::ltm
{
Base::operator=(other);
dbsettings = std::move(other.dbsettings);
- alwaysTransferSettings = std::move(other.alwaysTransferSettings);
periodicTransferSettings = std::move(other.periodicTransferSettings);
onFullTransferSettings = std::move(other.onFullTransferSettings);
reloaded = other.reloaded;
@@ -69,7 +65,7 @@ namespace armarx::armem::ltm
ARMARX_WARNING << deactivateSpam("ConnectionIsNotValid")
<< "The connection to mongocxx for memory '" << name() << "' is not valid. Settings are: " << dbsettings.toString()
<< "\nTo start it, run e.g.: \n"
- << "mongod --port " << dbsettings.port << " --dbpath \"/tmp/\""
+ << "armarx memory start"
<< "\n\n";
return false;
}
@@ -83,9 +79,8 @@ namespace armarx::armem::ltm
defs->optional(dbsettings.user, prefix + "ltm.20_user");
defs->optional(dbsettings.password, prefix + "ltm.21_password");
defs->optional(dbsettings.database, prefix + "ltm.22_database");
- defs->optional(alwaysTransferSettings.enabled, prefix + "ltm.30_enableAlwaysTransfer", "Enable transfer whenever new data is committed (This disables the other transfer modes!).");
- defs->optional(periodicTransferSettings.enabled, prefix + "ltm.31_enablePeriodicTransfer", "Enable transfer based on periodic interval.");
- defs->optional(onFullTransferSettings.enabled, prefix + "ltm.32_enableOnFullTransfer", "Enable transfer whenever the wm is full (see maxHistorySize).");
+ defs->optional(periodicTransferSettings.enabled, prefix + "ltm.30_enablePeriodicTransfer", "Enable transfer based on periodic interval.");
+ defs->optional(onFullTransferSettings.enabled, prefix + "ltm.31_enableOnFullTransfer", "Enable transfer whenever the wm is full (see maxHistorySize).");
}
wm::Memory Memory::convert() const
@@ -230,7 +225,6 @@ namespace armarx::armem::ltm
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 b24f4a7a662e2b5980d3dec00ecbb39b2c972673..4902701b33374c75affc2499d9ec93d3a18a9912 100644
--- a/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/longtermmemory/Memory.h
@@ -10,7 +10,6 @@
#include "../workingmemory/Memory.h"
// Properties
-#include <ArmarXCore/core/application/properties/PluginAll.h>
#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
namespace armarx::armem::ltm
@@ -33,11 +32,6 @@ namespace armarx::armem::ltm
bool enabled = false;
};
- struct AlwaysTransferSettings : public TransferSettings
- {
-
- };
-
struct PeriodicTransferSettings : public TransferSettings
{
bool deleteFromWMOnTransfer = false;
@@ -53,6 +47,7 @@ namespace armarx::armem::ltm
};
Mode mode;
+ int batch_size = 20;
};
@@ -92,7 +87,6 @@ namespace armarx::armem::ltm
public:
MongoDBConnectionManager::MongoDBSettings dbsettings;
- AlwaysTransferSettings alwaysTransferSettings;
PeriodicTransferSettings periodicTransferSettings;
OnFullTransferSettings onFullTransferSettings;
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
index 70fd41e7c451175d5964a81d48f4e8270c2e611c..3d0888c7ff4d2f1b46f54e9d51a1a3c62b7dd395 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.cpp
@@ -55,6 +55,11 @@ namespace armarx::armem::wm
}
+ void Memory::defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix)
+ {
+ }
+
+
// TODO: Add core segment if param is set
Memory::Base::UpdateResult
Memory::updateLocking(const EntityUpdate& update)
diff --git a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
index d52bce5ca30d2a464881e73f4cd4994a29429751..5093ffce2adddfcdd27e1f08529475335fc84ff2 100644
--- a/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
+++ b/source/RobotAPI/libraries/armem/core/workingmemory/Memory.h
@@ -1,10 +1,15 @@
#pragma once
+// Base Class
#include <RobotAPI/libraries/armem/core/base/MemoryBase.h>
+// ArmarX
#include "CoreSegment.h"
#include "detail/CopyWithoutData.h"
+// Properties
+#include <ArmarXCore/core/application/properties/PluginAll.h>
+#include <ArmarXCore/core/application/properties/PropertyDefinitionContainer.h>
namespace armarx::armem::wm
{
@@ -27,6 +32,9 @@ namespace armarx::armem::wm
Memory& operator=(const Memory& other) = default;
Memory& operator=(Memory&& other) = default;
+ // PropertyDefinitions related to LTM
+ void defineProperties(armarx::PropertyDefinitionsPtr defs, const std::string& prefix = "");
+
/**
* @brief Perform the commit, locking the core segments.
*
diff --git a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
index 354eec9cea9d1d1e37f4dc4ea790b47ba1749755..65bd2f6429c6fb2c25f48d2fa1218bc5fa308409 100644
--- a/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
+++ b/source/RobotAPI/libraries/armem/server/MemoryToIceAdapter.cpp
@@ -176,17 +176,7 @@ namespace armarx::armem::server
// TODO: Move outside of loop?
if (longtermMemory)
{
- if (longtermMemory->alwaysTransferSettings.enabled)
- {
- wm::Memory tmp(longtermMemory->name());
- tmp._addMissingCoreSegmentDuringUpdate = true;
- tmp.update(update);
- longtermMemory->append(tmp);
- }
- else
- {
- // TODO: see below
- }
+
}
// TODO: Consollidate to ltm if onFilledTransfer is enabled (fabian.peller)
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 469b5f907d01bbff757b1bea14ff8e80e5769315..0e3ddac45c6a8eee377f468767a3bc39a580c554 100644
--- a/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.h
+++ b/source/RobotAPI/libraries/armem/server/query_proc/diskmemory/BaseQueryProcessor.h
@@ -25,7 +25,7 @@ namespace armarx::armem::d_ltm::query_proc
query::data::QueryTarget getTargetType() const override
{
- return query::data::QueryTarget::Disk;
+ return query::data::QueryTarget::LTM;
}
};
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
index 5016db50bf5da8dc10d0fac5b5c004b9178ddc40..38e4d8c88719541e7c4fca92964cfb069c22ba5e 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.cpp
@@ -4,6 +4,7 @@
#include <QLineEdit>
#include <QTimer>
#include <QHBoxLayout>
+#include <QFileDialog>
#include <cmath>
@@ -16,32 +17,53 @@ namespace armarx::armem::gui
setSizePolicy(QSizePolicy::Policy::Minimum, QSizePolicy::Policy::Fixed);
auto vlayout = new QVBoxLayout();
- auto hlayout = new QHBoxLayout();
+ auto hlayout1 = new QHBoxLayout();
+ auto hlayout2 = new QHBoxLayout();
+
+ hlayout1->setContentsMargins(10, 2, 10, 2);
+ hlayout2->setContentsMargins(10, 2, 10, 2);
const int margin = 0;
vlayout->setContentsMargins(margin, margin, margin, margin);
_lineEdit = new QLineEdit("/tmp/MemoryExport", this);
_lineEdit->setSizePolicy(QSizePolicy::Ignored, QSizePolicy::Preferred);
- _lineEdit->setMinimumWidth(400);
+ _lineEdit->setMinimumWidth(150);
+
+ _openFileBrowserButton = new QPushButton("Search files", this);
+
+ _storeOnDiskButton = new QPushButton("Store query (Disk)", this);
+ _storeInLTMButton = new QPushButton("Store query (LTM)", this);
- _storeOnDiskButton = new QPushButton("Store current result on local Disk", this);
+ hlayout1->addWidget(_lineEdit);
+ hlayout1->addWidget(_openFileBrowserButton);
- _storeInLTMButton = new QPushButton("Store current result in LTM", this);
+ hlayout2->addWidget(_storeOnDiskButton);
+ hlayout2->addWidget(_storeInLTMButton);
- vlayout->addWidget(_lineEdit);
- hlayout->addWidget(_storeOnDiskButton);
- hlayout->addWidget(_storeInLTMButton);
- vlayout->addItem(hlayout);
+ vlayout->addItem(hlayout1);
+ vlayout->addItem(hlayout2);
this->setLayout(vlayout);
// Private connections.
// Public connections.
+ connect(_openFileBrowserButton, &QPushButton::pressed, this, &This::openFileBrowser);
connect(_storeInLTMButton, &QPushButton::pressed, this, &This::storeInLTM);
connect(_storeOnDiskButton, &QPushButton::pressed, this, &This::storeOnDisk);
}
+ void MemoryControlWidget::openFileBrowser()
+ {
+ QFileDialog dialog;
+ dialog.setFileMode(QFileDialog::DirectoryOnly);
+ //dialog.setOption(QFileDialog::DontUseNativeDialog, true);
+ dialog.setOption(QFileDialog::ShowDirsOnly, false);
+ dialog.exec();
+ QString open = dialog.directory().path();
+ _lineEdit->setText(open);
+ }
+
QLineEdit* MemoryControlWidget::pathInputBox()
{
return _lineEdit;
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
index 19831694acc7b9251c7d372b8381f755df1bc292..c6b156db313b01180a7f5884959ffc21b0a73f9d 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/MemoryControlWidget.h
@@ -4,6 +4,7 @@
class QPushButton;
class QLineEdit;
+class QFileDialog;
namespace armarx::armem::gui
{
@@ -20,11 +21,15 @@ namespace armarx::armem::gui
QLineEdit* pathInputBox();
QString getEnteredPath();
+ QPushButton* openFileBrowserButton();
+
QPushButton* storeInLTMButton();
QPushButton* storeOnDiskButton();
public slots:
+ void openFileBrowser();
+
signals:
void storeInLTM();
@@ -40,6 +45,8 @@ namespace armarx::armem::gui
QLineEdit* _lineEdit;
+ QPushButton* _openFileBrowserButton;
+
QPushButton* _storeInLTMButton;
QPushButton* _storeOnDiskButton;
diff --git a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
index f88a2e1731c87de6fa163c307c90f4c6d3e01ae2..4edb2b417bc9b40fde95643519701b4962aee114 100644
--- a/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
+++ b/source/RobotAPI/libraries/armem_gui/MemoryViewer.cpp
@@ -216,6 +216,8 @@ namespace armarx::armem::gui
void MemoryViewer::updateMemories()
{
+ int errorCnt = 0;
+
memoryReaders.clear();
memoryData.clear();
@@ -228,6 +230,8 @@ namespace armarx::armem::gui
std::filesystem::path p(utf8_text);
+ armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
+
// first check if the local file system should be queried
if (memoryGroup->queryWidget()->alsoQueryLocalDisk())
{
@@ -241,15 +245,13 @@ namespace armarx::armem::gui
armem::d_ltm::Memory dMem(k);
dMem.reload(p / k);
- armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
- input.addQueryTargetToAll(armem::query::data::QueryTarget::Disk);
+ input.addQueryTargetToAll(armem::query::data::QueryTarget::LTM); // We use LTM as query target for the disk
armem::d_ltm::query_proc::MemoryQueryProcessor d_ltm_processor;
dMem = d_ltm_processor.process(input.toIce(), dMem);
wm::Memory converted = dMem.convert();
memoryData[k] = std::move(converted);
- dataChanged = true;
}
}
}
@@ -257,67 +259,78 @@ namespace armarx::armem::gui
{
ARMARX_WARNING << "Could not import a memory from '" << utf8_text << "'. Skipping import.";
}
- }
- armem::client::QueryInput input = memoryGroup->queryWidget()->queryInput();
- for (auto& [name, reader] : memoryReaders)
+ dataChanged = true; // in any case we update the view (even if there was no result)
+ }
+ else
{
- TIMING_START(MemoryQuery);
+
+ for (auto& [name, reader] : memoryReaders)
{
- armem::client::QueryResult result = reader.query(input);
- if (result)
+ TIMING_START(MemoryQuery);
{
- if (result.memory.hasData())
+ armem::client::QueryResult result = reader.query(input);
+ if (result)
{
- if (const auto& it = memoryData.find(name); it != memoryData.end())
+ if (result.memory.hasData())
{
- result.memory.append(it->second);
-
- // 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);
-
- if (!result.memory.hasData())
+ if (const auto& it = memoryData.find(name); it != memoryData.end())
{
- ARMARX_ERROR << "A memory which had data before lost data. This indicates that there is something wrong.";
+ // TODO is this necessary (or dead code?)
+ result.memory.append(it->second);
+
+ if (!result.memory.hasData())
+ {
+ ARMARX_ERROR << "A memory which had data before lost data. This indicates that there is something wrong.";
+ }
}
- }
- dataChanged = true;
- memoryData[name] = std::move(result.memory);
+ memoryData[name] = std::move(result.memory);
+ }
+ else
+ {
+ ARMARX_INFO << "The memory " << name << " has no data after querying.";
+ }
+ dataChanged = true; // in any case we update the view (even if there was no result)
}
else
{
- ARMARX_INFO << "The memory " << name << " has no data after querying.";
+ ARMARX_WARNING << "A query for memory '" << name << "' produced an error: " << result.errorMessage;
+ errorCnt++;
}
}
- else
+ TIMING_END_STREAM(MemoryQuery, ARMARX_VERBOSE);
+
+ if (debugObserver)
{
- ARMARX_WARNING << "A query for memory '" << name << "' produced an error: " << result.errorMessage;
- if (statusLabel)
- {
- statusLabel->setText(QString::fromStdString(result.errorMessage));
- }
+ debugObserver->setDebugDatafield(Logging::tag.tagName, "Memory Query [ms]", new Variant(MemoryQuery.toMilliSecondsDouble()));
}
}
- TIMING_END_STREAM(MemoryQuery, ARMARX_VERBOSE);
-
- if (debugObserver)
- {
- debugObserver->setDebugDatafield(Logging::tag.tagName, "Memory Query [ms]", new Variant(MemoryQuery.toMilliSecondsDouble()));
- }
}
+ // Code to output status label information
+ std::stringstream ss;
+ auto now = std::chrono::system_clock::now();
+ auto in_time_t = std::chrono::system_clock::to_time_t(now);
+ ss << "Last update: " << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d %X");
+
if (dataChanged)
{
emit memoryDataChanged();
}
else
{
- if (statusLabel)
- {
- statusLabel->setText("No query result.");
- }
+ ss << "- No query result. ";
+ }
+
+ if (errorCnt > 0)
+ {
+ ss << "The query produced " << errorCnt << " errors! Please check log.";
+ }
+
+ if (statusLabel)
+ {
+ statusLabel->setText(ss.str().c_str());
}
}
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
index e41bed15cecbc599c7c97b4266a04e0e9a70e47e..9d5cc9ea6064bb6977892623649518791dcaffec 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.cpp
@@ -26,11 +26,23 @@ namespace armarx::armem::gui
{
targets.push_back(query::data::QueryTarget::LTM);
}
- if (_DiskMemoryQueryTargetCheckBox->isChecked())
+ return targets;
+ }
+
+ void SnapshotSelectorWidget::localDiskStateChanged()
+ {
+ if (_LocalDiskMemoryQueryTargetCheckBox->isChecked())
{
- targets.push_back(query::data::QueryTarget::Disk);
+ _WMQueryTargetCheckBox->setChecked(false);
+ _LTMQueryTargetCheckBox->setChecked(false);
+ _WMQueryTargetCheckBox->setEnabled(false);
+ _LTMQueryTargetCheckBox->setEnabled(false);
+ }
+ else
+ {
+ _WMQueryTargetCheckBox->setEnabled(true);
+ _LTMQueryTargetCheckBox->setEnabled(true);
}
- return targets;
}
bool SnapshotSelectorWidget::queryTargetContainsLocalDisk() const
@@ -59,13 +71,13 @@ namespace armarx::armem::gui
// query type select box
auto queryTargetLayout = new QHBoxLayout();
_WMQueryTargetCheckBox = new QCheckBox("WM");
- _LTMQueryTargetCheckBox = new QCheckBox("LTM (MongoDB)");
- _DiskMemoryQueryTargetCheckBox = new QCheckBox("LTM (Disk)");
+ _LTMQueryTargetCheckBox = new QCheckBox("LTM");
_LocalDiskMemoryQueryTargetCheckBox = new QCheckBox("Local Disk");
+ connect(_LocalDiskMemoryQueryTargetCheckBox, &QCheckBox::stateChanged, this, &This::localDiskStateChanged);
+
queryTargetLayout->addWidget(_WMQueryTargetCheckBox);
queryTargetLayout->addWidget(_LTMQueryTargetCheckBox);
- queryTargetLayout->addWidget(_DiskMemoryQueryTargetCheckBox);
queryTargetLayout->addWidget(_LocalDiskMemoryQueryTargetCheckBox);
_WMQueryTargetCheckBox->setChecked(true);
diff --git a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
index 6f6422c3cdbc7fd0e1899bce47efbaaa32589062..a0b69eb1b6f362e64880ab400c0bea29bcdc73fc 100644
--- a/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
+++ b/source/RobotAPI/libraries/armem_gui/query_widgets/SnapshotSelectorWidget.h
@@ -45,14 +45,13 @@ namespace armarx::armem::gui
signals:
void queryChanged();
-
private slots:
//void updateSelector();
void hideAllForms();
void showSelectedFormForQuery(QString selected);
-
+ void localDiskStateChanged();
signals:
void queryOutdated();
@@ -68,7 +67,6 @@ namespace armarx::armem::gui
QComboBox* _queryComboBox;
QCheckBox* _WMQueryTargetCheckBox;
QCheckBox* _LTMQueryTargetCheckBox;
- QCheckBox* _DiskMemoryQueryTargetCheckBox;
QCheckBox* _LocalDiskMemoryQueryTargetCheckBox;
/// The forms for the different query types. Hidden when not selected.
std::map<QString, SnapshotForm*> _queryForms;