Add synchronization in onConnect (to be sure)

source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.cpp

@@ -147,7 +147,7 @@ void KinematicUnitWidgetController::onInitComponent()
 
 void KinematicUnitWidgetController::onConnectComponent()
 {
-    ARMARX_INFO << "kinematic unit gui :: onConnectComponent()";
+    // ARMARX_INFO << "Kinematic Unit Gui :: onConnectComponent()";
     reportedJointAngles.clear();
     reportedJointVelocities.clear();
     reportedJointControlModes.clear();
@@ -156,10 +156,11 @@ void KinematicUnitWidgetController::onConnectComponent()
     reportedJointStatuses.clear();
     reportedJointTemperatures.clear();
 
-
     jointAnglesUpdateFrequency = new filters::MedianFilter(100);
     kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
     topicName = kinematicUnitInterfacePrx->getReportTopicName();
+    usingTopic(topicName);
+
     lastJointAngleUpdateTimestamp = TimeUtil::GetTime().toSecondsDouble();
     robotVisu->removeAllChildren();
 
@@ -168,10 +169,9 @@ void KinematicUnitWidgetController::onConnectComponent()
     std::string rfile;
     Ice::StringSeq includePaths;
 
-    // get robot filename
+    // Get robot filename
     try
     {
-
         Ice::StringSeq packages = kinematicUnitInterfacePrx->getArmarXPackages();
         packages.push_back(Application::GetProjectName());
         ARMARX_VERBOSE << "ArmarX packages " << packages;
@@ -200,7 +200,7 @@ void KinematicUnitWidgetController::onConnectComponent()
     }
     catch (...)
     {
-        ARMARX_ERROR << "Unable to retrieve robot filename";
+        ARMARX_ERROR << "Unable to retrieve robot filename.";
     }
 
     try
@@ -208,6 +208,10 @@ void KinematicUnitWidgetController::onConnectComponent()
         ARMARX_INFO << "Loading robot from file " << rfile;
         robot = loadRobotFile(rfile);
     }
+    catch (const std::exception& e)
+    {
+        ARMARX_ERROR << "Failed to init robot: " << e.what();
+    }
     catch (...)
     {
         ARMARX_ERROR << "Failed to init robot";
@@ -216,35 +220,34 @@ void KinematicUnitWidgetController::onConnectComponent()
     if (!robot || !robot->hasRobotNodeSet(robotNodeSetName))
     {
         getObjectScheduler()->terminate();
-
         if (getWidget()->parentWidget())
         {
             getWidget()->parentWidget()->close();
         }
-
-        std::cout << "returning" << std::endl;
         return;
     }
 
-    // check robot name and disable setZero Button if necessary
+    // Check robot name and disable setZero Button if necessary
     if (not simox::alg::starts_with(robot->getName(), "Armar3"))
     {
-        ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is " << robot->getName();
+        ARMARX_IMPORTANT << "Disable the SetZero button because the robot name is '" << robot->getName() << "'.";
         ui.pushButtonKinematicUnitPos1->setDisabled(true);
     }
 
     kinematicUnitFile = rfile;
     robotNodeSet = robot->getRobotNodeSet(robotNodeSetName);
 
-
-    initGUIComboBox(robotNodeSet); // init the pull down menu (QT: ComboBox)
-    initGUIJointListTable(robotNodeSet);
-
     kinematicUnitVisualization = getCoinVisualization(robot);
     kinematicUnitNode = kinematicUnitVisualization->getCoinVisualization();
     robotVisu->addChild(kinematicUnitNode);
 
-    // init control mode map
+    // Fetch the current joint angles.
+    synchronizeRobotJointAngles();
+
+    initGUIComboBox(robotNodeSet);  // init the pull down menu (QT: ComboBox)
+    initGUIJointListTable(robotNodeSet);
+
+    // Init control mode map
     try
     {
         reportedJointControlModes = kinematicUnitInterfacePrx->getControlModes();
@@ -255,7 +258,6 @@ void KinematicUnitWidgetController::onConnectComponent()
 
     initializeUi();
 
-    usingTopic(topicName);
     QMetaObject::invokeMethod(this, "resetSlider");
     enableMainWidgetAsync(true);
     updateTimerId = startTimer(1000); // slow timer that only ensures that skipped updates are shown at all
@@ -355,13 +357,11 @@ void KinematicUnitWidgetController::saveSettings(QSettings* settings)
     settings->setValue("enableValueValidator", enableValueValidator);
     settings->setValue("viewerEnabled", viewerEnabled);
     assert(historyTime % 1000 == 0);
-    settings->setValue("historyTime", (int)(historyTime / 1000));
+    settings->setValue("historyTime", static_cast<int>(historyTime / 1000));
     settings->setValue("currentValueMax", currentValueMax);
 }
 
 
-
-
 void KinematicUnitWidgetController::showVisuLayers(bool show)
 {
     if (debugDrawer)
@@ -410,7 +410,9 @@ void KinematicUnitWidgetController::updateGuiElements()
     // modelUpdateCB();
 }
 
-void KinematicUnitWidgetController::updateKinematicUnitListInDialog() {}
+void KinematicUnitWidgetController::updateKinematicUnitListInDialog()
+{
+}
 
 void KinematicUnitWidgetController::modelUpdateCB()
 {
@@ -418,7 +420,6 @@ void KinematicUnitWidgetController::modelUpdateCB()
 
 SoNode* KinematicUnitWidgetController::getScene()
 {
-
     if (viewerEnabled)
     {
         ARMARX_INFO << "Returning scene ";
@@ -500,7 +501,7 @@ void KinematicUnitWidgetController::kinematicUnitZeroPosition()
     {
     }
 
-    //set slider to 0 if position control mode is used
+    // Set slider to 0 if position control mode is used.
     if (selectedControlMode == ePositionControl)
     {
         ui.horizontalSliderKinematicUnitPos->setSliderPosition(SLIDER_ZERO_POSITION);
@@ -615,9 +616,9 @@ void KinematicUnitWidgetController::setControlModePosition()
 
     if (currentNode)
     {
-        QString unit = currentNode->isRotationalJoint() ?
-                       (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad") :
-                       "mm";
+        QString unit = currentNode->isRotationalJoint()
+                ? (ui.checkBoxUseDegree->isChecked() ? "deg" : "rad")
+                : "mm";
         ui.labelUnit->setText(unit);
         const bool isDeg = ui.checkBoxUseDegree->isChecked();
         const bool isRot = currentNode->isRotationalJoint();
@@ -644,9 +645,7 @@ void KinematicUnitWidgetController::setControlModePosition()
             // Therefore, we first have to fetch the actual joint values and use that one.
             // However, this should actually not be necessary, as the robot model should be updated
             // via the topics.
-
-            const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
-            robot->setJointValues(currentJointAngles);
+            synchronizeRobotJointAngles();
         }
 
         float pos = currentNode->getJointValue() * conversionFactor;
@@ -745,10 +744,7 @@ void KinematicUnitWidgetController::setControlModeTorque()
 
         ui.horizontalSliderKinematicUnitPos->blockSignals(false);
         resetSlider();
-
     }
-
-
 }
 
 VirtualRobot::RobotPtr KinematicUnitWidgetController::loadRobotFile(std::string fileName)
@@ -1729,3 +1725,9 @@ void KinematicUnitWidgetController::on_pushButtonFromJson_clicked()
         ARMARX_ERROR << "failed to switch mode or set angles";
     }
 }
+
+void KinematicUnitWidgetController::synchronizeRobotJointAngles()
+{
+    const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+    robot->setJointValues(currentJointAngles);
+}

source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitGuiPlugin.h

@@ -268,9 +268,13 @@ namespace armarx
         void highlightCriticalValues();
 
     protected slots:
+
         void showVisuLayers(bool show);
         void copyToClipboard();
         void on_pushButtonFromJson_clicked();
+
+        void synchronizeRobotJointAngles();
+
     private:
 
         std::recursive_mutex mutexNodeSet;