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/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;