changes for demo tomorrow

source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.cpp

@@ -86,45 +86,52 @@ QPointer<QDialog> HapticUnitWidget::getConfigDialog(QWidget* parent)
 }
 
 
- void HapticUnitWidget::configured()
- {
-     //HapticUnitProxyName = dialog->ui->editHapticUnitName->text().toStdString();
-     //handName = dialog->ui->editHandName->text().toStdString();
- }
-
- void HapticUnitWidget::updateData()
- {
-     /*//::armarx::ChannelRegistryEntry channel = HapticObserverProxy->getAvailableChannels().at("A");
-     //float max = HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, "A", "max"))->getFloat();
-     MatrixFloatPtr matrix1 = VariantPtr::dynamicCast(HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, "A", "matrix")))->get<MatrixFloat>();
-     MatrixFloatPtr matrix2 = VariantPtr::dynamicCast(HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, "B", "matrix")))->get<MatrixFloat>();
-
-     //::armarx::DataFieldRegistryEntry dataFieldMax = channel.dataFields.at("max");
-     //float max = dataFieldMax.value->getFloat();
-     //::armarx::DataFieldRegistryEntry dataFieldMatrix = channel.dataFields.at("matrix");
-
-     //MatrixFloatPtr matrix = VariantPtr::dynamicCast(dataFieldMatrix.value)->get<MatrixFloat>();
-
-     //ARMARX_LOG << max;
-     //ARMARX_LOG << matrix1->toEigen();
-     matrixDisplays.at(0)->setData(matrix1->toEigen().cast<double>());
-     matrixDisplays.at(0)->invokeUpdate();
-     matrixDisplays.at(1)->setData(matrix2->toEigen().cast<double>());
-     matrixDisplays.at(1)->invokeUpdate();*/
-
-     for(std::map<std::string,MatrixDisplayWidget*>::iterator it = matrixDisplays.begin(); it != matrixDisplays.end(); it++)
-     {
-         MatrixFloatPtr matrix = VariantPtr::dynamicCast(HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, it->first, "matrix")))->get<MatrixFloat>();
-         std::string name = HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, it->first, "name"))->getString();
-         float rate = HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, it->first, "rate"))->getFloat();
-         TimestampVariantPtr timestamp = VariantPtr::dynamicCast(HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, it->first, "timestamp")))->get<TimestampVariant>();
-         MatrixDisplayWidget* matrixDisplay = it->second;
-         matrixDisplay->setData(matrix->toEigen().cast<double>());
-         matrixDisplay->setInfoOverlay(QString::fromStdString(it->first) + ": " + QString::fromStdString(name) + " " + QString::fromStdString(timestamp->toTime().toDateTime()) + " " + QString::number(rate, 'g', 4) + " Frames/s");
-         matrixDisplay->invokeUpdate();
-     }
-
- }
+void HapticUnitWidget::configured()
+{
+    //HapticUnitProxyName = dialog->ui->editHapticUnitName->text().toStdString();
+    //handName = dialog->ui->editHandName->text().toStdString();
+}
+
+void HapticUnitWidget::updateData()
+{
+    /*//::armarx::ChannelRegistryEntry channel = HapticObserverProxy->getAvailableChannels().at("A");
+    //float max = HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, "A", "max"))->getFloat();
+    MatrixFloatPtr matrix1 = VariantPtr::dynamicCast(HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, "A", "matrix")))->get<MatrixFloat>();
+    MatrixFloatPtr matrix2 = VariantPtr::dynamicCast(HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, "B", "matrix")))->get<MatrixFloat>();
+
+    //::armarx::DataFieldRegistryEntry dataFieldMax = channel.dataFields.at("max");
+    //float max = dataFieldMax.value->getFloat();
+    //::armarx::DataFieldRegistryEntry dataFieldMatrix = channel.dataFields.at("matrix");
+
+    //MatrixFloatPtr matrix = VariantPtr::dynamicCast(dataFieldMatrix.value)->get<MatrixFloat>();
+
+    //ARMARX_LOG << max;
+    //ARMARX_LOG << matrix1->toEigen();
+    matrixDisplays.at(0)->setData(matrix1->toEigen().cast<double>());
+    matrixDisplays.at(0)->invokeUpdate();
+    matrixDisplays.at(1)->setData(matrix2->toEigen().cast<double>());
+    matrixDisplays.at(1)->invokeUpdate();*/
+
+    for(std::map<std::string,MatrixDisplayWidget*>::iterator it = matrixDisplays.begin(); it != matrixDisplays.end(); it++)
+    {
+        MatrixFloatPtr matrix = VariantPtr::dynamicCast(HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, it->first, "matrix")))->get<MatrixFloat>();
+        std::string name = HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, it->first, "name"))->getString();
+        float rate = HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, it->first, "rate"))->getFloat();
+        TimestampVariantPtr timestamp = VariantPtr::dynamicCast(HapticObserverProxy->getDataField(new DataFieldIdentifier(HapticObserverProxyName, it->first, "timestamp")))->get<TimestampVariant>();
+        MatrixDisplayWidget* matrixDisplay = it->second;
+        matrixDisplay->setData(matrix->toEigen().cast<double>());
+        matrixDisplay->setInfoOverlay(QString::fromStdString(it->first) + ": " + QString::fromStdString(name) + " " + QString::fromStdString(timestamp->toTime().toDateTime()) + " " + QString::number(rate, 'g', 4) + " Frames/s");
+        matrixDisplay->invokeUpdate();
+    }
+
+}
+
+void HapticUnitWidget::onContextMenu(QPoint pos)
+{
+    //QMenu* menu = new
+
+    //contextMenuFolder->exec(treeView->mapToGlobal(point));
+}
 
 
 void HapticUnitWidget::loadSettings(QSettings *settings)
@@ -166,6 +173,7 @@ void HapticUnitWidget::updateDisplayWidgets()
         ARMARX_LOG << it->first;
         MatrixDisplayWidget* matrixDisplay = new MatrixDisplayWidget();
         matrixDisplay->setRange(0, 4095);
+        connect(matrixDisplay, SIGNAL(customContextMenuRequested(QPoint)), this, SLOT(onContextMenu(QPoint)));
         matrixDisplays[it->first] = matrixDisplay;
         layout->addWidget(matrixDisplay, 0, i);
         i++;

source/RobotAPI/gui_plugins/HapticUnitPlugin/HapticUnitGuiPlugin.h

@@ -98,6 +98,7 @@ namespace armarx
 
     public slots:
         void updateData();
+        void onContextMenu(QPoint pos);
 
     signals:
         void doUpdateDisplayWidgets();

source/RobotAPI/interface/CMakeLists.txt

@@ -20,6 +20,7 @@ set(SLICE_FILES
     units/ForceTorqueUnit.ice
     units/HandUnitInterface.ice
     units/HapticUnit.ice
+    units/WeissHapticUnit.ice
     units/HeadIKUnit.ice
     units/KinematicUnitInterface.ice
     units/PlatformUnitInterface.ice

source/RobotAPI/libraries/drivers/WeissHapticSensor/TactileSensor.cpp

@@ -265,8 +265,22 @@ string TactileSensor::getDeviceTag()
     Response response = interface->submitCmd( 0x52, payload, sizeof(payload), false );
     response.ensureMinLength(2);
     response.ensureSuccess();
-    std::string type = std::string((char*)response.data.data() + 2, response.len - 2);
-    return type;
+    std::string tag = std::string((char*)response.data.data() + 2, response.len - 2);
+    return tag;
+}
+
+bool TactileSensor::tryGetDeviceTag(string &tag)
+{
+    unsigned char payload[0];
+    Response response = interface->submitCmd( 0x52, payload, sizeof(payload), false );
+    response.ensureMinLength(2);
+    if (response.status == E_NOT_AVAILABLE)
+    {
+        return false;
+    }
+    response.ensureSuccess();
+    tag = std::string((char*)response.data.data() + 2, response.len - 2);
+    return true;
 }
 int TactileSensor::loop( char *data, int data_len ) { return 0; }
 

source/RobotAPI/libraries/drivers/WeissHapticSensor/TactileSensor.h

@@ -85,6 +85,7 @@ public:
     static void printSystemInformation(tac_system_information_t si);
     void setDeviceTag(string tag);
     string getDeviceTag();
+    bool tryGetDeviceTag(string &tag);
     int loop( char *data, int data_len );
 
     string getInterfaceInfo();

source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.cpp

@@ -36,7 +36,15 @@ void WeissHapticSensor::connect()
     //TactileSensor::printSystemInformation(si);
 
     //sensor->setDeviceTag("Test Tag");
-    this->tag = sensor->getDeviceTag();
+    string tag;
+    if(sensor->tryGetDeviceTag(tag))
+    {
+        this->tag = tag;
+    }
+    else
+    {
+        this->tag = deviceFileName;
+    }
     //cout << boost::format("Sensor Tag = %1%") % tag << endl;
 
     jsWriter->writeLine(str(boost::format("%s = {};") % deviceFileName));
@@ -79,6 +87,18 @@ void WeissHapticSensor::startSampling()
     sensorTask->start();
 }
 
+string WeissHapticSensor::getDeviceName()
+{
+    return device;
+}
+
+void WeissHapticSensor::scheduleSetDeviceTag(string tag)
+{
+    boost::mutex::scoped_lock lock(mutex);
+    setDeviceTagValue = tag;
+    setDeviceTagScheduled = true;
+}
+
 void WeissHapticSensor::frameAcquisitionTaskLoop()
 {
     cout << this << ": readAndReportSensorValues" << endl;
@@ -117,6 +137,23 @@ void WeissHapticSensor::frameAcquisitionTaskLoop()
         {
             ARMARX_WARNING << "Caught ChecksumErrorException on " << device << ", skipping frame";
         }
+
+        if(setDeviceTagScheduled)
+        {
+            boost::mutex::scoped_lock lock(mutex);
+            setDeviceTagScheduled = false;
+
+            ARMARX_INFO << "[" << device << "] Stopping periodic frame aquisition to set new device tag";
+            sensor->stopPeriodicFrameAcquisition();
+
+            ARMARX_INFO << "[" << device << "] Setting new device tag '" << setDeviceTagValue << "'";
+            sensor->setDeviceTag(setDeviceTagValue);
+
+            ARMARX_INFO << "[" << device << "] Starting periodic frame aquisition";
+            sensor->startPeriodicFrameAcquisition(0);
+
+            setDeviceTagScheduled = false;
+        }
         //usleep(3000);
 
         //usleep(3000);

source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensor.h

@@ -11,9 +11,14 @@
 #include "TextWriter.h"
 #include <Core/observers/variant/TimestampVariant.h>
 #include <Core/util/variants/eigen3/MatrixVariant.h>
+#include <boost/thread/mutex.hpp>
 
 namespace armarx
 {
+
+    class WeissHapticSensor;
+    typedef boost::shared_ptr<WeissHapticSensor> WeissHapticSensorPtr;
+
     class WeissHapticSensor : public Logging
     {
     public:
@@ -25,6 +30,9 @@ namespace armarx
         void setListenerPrx(HapticUnitListenerPrx listenerPrx);
         void startSampling();
 
+        std::string getDeviceName();
+        void scheduleSetDeviceTag(std::string tag);
+
     private:
         RunningTask<WeissHapticSensor>::pointer_type sensorTask;
         boost::shared_ptr<TextWriter> jsWriter;
@@ -38,6 +46,11 @@ namespace armarx
         tac_matrix_info_t mi;
         HapticUnitListenerPrx listenerPrx;
         void writeMatrixToJs(MatrixFloatPtr matrix, TimestampVariantPtr timestamp);
+
+        bool setDeviceTagScheduled;
+        std::string setDeviceTagValue;
+
+        boost::mutex mutex;
     };
 }
 

source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensorsUnit.cpp

@@ -69,7 +69,7 @@ void WeissHapticSensorsUnit::onInitHapticUnit()
 
     for(std::vector<std::string>::iterator it = devices.begin(); it != devices.end(); ++it)
     {
-        boost::shared_ptr<WeissHapticSensor> sensor(new WeissHapticSensor(*it));
+        WeissHapticSensorPtr sensor(new WeissHapticSensor(*it));
         this->sensors.push_back(sensor);
     }
 
@@ -117,6 +117,27 @@ std::vector< std::string > WeissHapticSensorsUnit::getDevices()
     return files;
 }
 
+void WeissHapticSensorsUnit::setDeviceTag(const string &deviceName, const string &tag, const Ice::Current &)
+{
+    for(WeissHapticSensorPtr sensor : sensors)
+    {
+        if(sensor->getDeviceName() == deviceName)
+        {
+            sensor->scheduleSetDeviceTag(tag);
+        }
+    }
+}
+
+void WeissHapticSensorsUnit::startLogging(const Ice::Current &)
+{
+    // @@@ TODO NotImplemented
+}
+
+void WeissHapticSensorsUnit::stopLogging(const Ice::Current &)
+{
+    // @@@ TODO NotImplemented
+}
+
 void WeissHapticSensorsUnit::onStartHapticUnit()
 {
 
@@ -143,7 +164,6 @@ void WeissHapticSensorsUnit::onExitHapticUnit()
 
 void WeissHapticSensorsUnit::onDisconnectComponent()
 {
-
 }
 
 

source/RobotAPI/libraries/drivers/WeissHapticSensor/WeissHapticSensorsUnit.h

@@ -25,7 +25,7 @@
 #define ARMAR4_FT_UNIT_ARMAR4_H
 
 #include <RobotAPI/components/units/HapticUnit.h>
-//#include <Core/interface/units/
+#include <RobotAPI/interface/units/WeissHapticUnit.h>
 #include <Core/core/system/ImportExportComponent.h>
 
 #include <string>
@@ -42,7 +42,9 @@ namespace armarx
             }
     };
     
-    class WeissHapticSensorsUnit : virtual public HapticUnit
+    class WeissHapticSensorsUnit :
+            virtual public WeissHapticUnitInterface,
+            virtual public HapticUnit
     {
         public:
             virtual std::string getDefaultName() { return "WeissHapticSensorsUnit"; }
@@ -71,6 +73,12 @@ namespace armarx
             std::vector< std::string > getDevices();
 
             std::vector< boost::shared_ptr< WeissHapticSensor > > sensors;
+
+            // WeissHapticUnitInterface interface
+    public:
+            void setDeviceTag(const string &deviceName, const string &tag, const Ice::Current &);
+            void startLogging(const Ice::Current &);
+            void stopLogging(const Ice::Current &);
     };
 }