Added timer to update current tcp pose and desired tcp pose in the text fields

source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp

@@ -35,7 +35,8 @@
 #include <VirtualRobot/XML/RobotIO.h>
 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
 
-#define TIMER_MS 33
+#define ROBOT_UPDATE_TIMER_MS 33
+#define TEXTFIELD_UPDATE_TIMER_MS 200
 
 armarx::RobotIKGuiPlugin::RobotIKGuiPlugin()
 {
@@ -103,9 +104,14 @@ void armarx::RobotIKWidgetController::onConnectComponent()
 
     //Make a timer update robo pose every time tick
     SoSensorManager* sensor_mgr = SoDB::getSensorManager();
-    timerSensor = new SoTimerSensor(timerCB, this);
-    timerSensor->setInterval(SbTime(TIMER_MS / 1000.0f));
-    sensor_mgr->insertTimerSensor(timerSensor);
+    robotUpdateSensor = new SoTimerSensor(robotUpdateTimerCB, this);
+    robotUpdateSensor->setInterval(SbTime(ROBOT_UPDATE_TIMER_MS / 1000.0f));
+    sensor_mgr->insertTimerSensor(robotUpdateSensor);
+
+    //And a timer to update the labels
+    textFieldUpdateSensor = new SoTimerSensor(textFieldUpdateTimerCB, this);
+    textFieldUpdateSensor->setInterval(SbTime(TEXTFIELD_UPDATE_TIMER_MS / 1000.0f));
+    sensor_mgr->insertTimerSensor(textFieldUpdateSensor);
 
     //Get all kinematic chain descriptions and add them to the combo box
     auto robotNodeSets = robot->getRobotNodeSets();
@@ -224,7 +230,7 @@ void armarx::RobotIKWidgetController::connectSlots()
     connect(ui.comboBox, SIGNAL(currentIndexChanged(QString)), this, SLOT(kinematicChainChanged(QString)), Qt::UniqueConnection);
 }
 
-void armarx::RobotIKWidgetController::timerCB(void *data, SoSensor *sensor)
+void armarx::RobotIKWidgetController::robotUpdateTimerCB(void *data, SoSensor *sensor)
 {
     RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
     if (!controller)
@@ -235,11 +241,13 @@ void armarx::RobotIKWidgetController::timerCB(void *data, SoSensor *sensor)
 
         if(controller->currentSeparator) {
             Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
+            //Apply tcp position to indicator
             tcpMatrix(0, 3) /= 1000;
             tcpMatrix(1, 3) /= 1000;
             tcpMatrix(2, 3) /= 1000;
             controller->tcpCurrentTransform->setMatrix(VirtualRobot::CoinVisualizationFactory::getSbMatrix(tcpMatrix));
         }
+
         if(controller->startUpCameraPositioning) {
             controller->ui.robotViewer->getRobotViewer()->cameraViewAll();
             controller->startUpCameraPositioning = false;
@@ -247,6 +255,47 @@ void armarx::RobotIKWidgetController::timerCB(void *data, SoSensor *sensor)
     } catch (...){};
 }
 
+void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void *data, SoSensor *sensor)
+{
+    RobotIKWidgetController* controller = static_cast<RobotIKWidgetController*>(data);
+    if (!controller)
+        return;
+
+    if(controller->currentSeparator) {
+        Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose();
+
+        //Set text label to tcp matrix
+        std::stringstream buffer;
+        buffer << tcpMatrix;
+        std::string matrixText = buffer.str();
+        controller->ui.currentPoseMatrix->setText(QString::fromStdString(matrixText));
+
+        //Set text label for desired tcp pose
+        //Therefore calculate current manipulator pose
+        SoTransform* tempTransform = new SoTransform;
+        tempTransform->ref();
+        tempTransform->combineRight(controller->tcpManip);
+        tempTransform->combineRight(controller->tcpManipTransform);
+        SbRotation tempRotation = tempTransform->rotation.getValue();
+        SbVec3f tempTranslation = tempTransform->translation.getValue();
+        tempTransform->unref();
+        float x = tempTranslation[0] * 1000;
+        float y = tempTranslation[1] * 1000;
+        float z = tempTranslation[2] * 1000;
+
+        Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
+        mat (0,3) = x;
+        mat (1,3) = y;
+        mat (2,3) = z;
+
+        //Set text label to manip matrix
+        std::stringstream buffer2;
+        buffer2 << mat;
+        std::string matrixText2 = buffer2.str();
+        controller->ui.desiredPoseMatrix->setText(QString::fromStdString(matrixText2));
+    }
+}
+
 void armarx::RobotIKWidgetController::kinematicChainChanged(const QString &arg1)
 {
     this->ui.moveTCP->setEnabled(true);

source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.h

@@ -121,8 +121,11 @@ namespace armarx
             SoMaterial* tcpCurrentColor;
             SoSphere* tcpCurrent;
 
-            SoTimerSensor* timerSensor;
-            static void timerCB(void *data, SoSensor *sensor);
+            SoTimerSensor* robotUpdateSensor;
+            static void robotUpdateTimerCB(void *data, SoSensor *sensor);
+
+            SoTimerSensor* textFieldUpdateSensor;
+            static void textFieldUpdateTimerCB(void *data, SoSensor *sensor);
 
             static void manipFinishCallback(void *data, SoDragger* manip);
 

source/RobotAPI/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.ui

@@ -154,6 +154,11 @@
            <verstretch>0</verstretch>
           </sizepolicy>
          </property>
+         <property name="font">
+          <font>
+           <pointsize>13</pointsize>
+          </font>
+         </property>
          <property name="text">
           <string>Current pose of TCP:</string>
          </property>
@@ -202,6 +207,11 @@
            <verstretch>0</verstretch>
           </sizepolicy>
          </property>
+         <property name="font">
+          <font>
+           <pointsize>13</pointsize>
+          </font>
+         </property>
          <property name="text">
           <string>Desired pose of TCP:</string>
          </property>