Fix bug: After a reconnect to robot/kinematic unit, local joint values can be...

Fix bug: After a reconnect to robot/kinematic unit, local joint values can be wrong. Need to fetch the real joint values first to avoid large jumps when selecting the joint.

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

@@ -638,8 +638,26 @@ void KinematicUnitWidgetController::setControlModePosition()
             return;
         }
 
+        // currentNode->getJointValue() can we wrong after we re-connected to the robot unit.
+        // E.g., it can be 0 although the torso joint was at -365 before the unit disconnected.
+        // Therefore, we first have to fetch the actual joint values and use that one.
+        const NameValueMap currentJointAngles = kinematicUnitInterfacePrx->getJointAngles();
+        if (auto it = currentJointAngles.find(currentNode->getName()); it != currentJointAngles.end())
+        {
+            currentNode->setJointValue(it->second);
+        }
+        else
+        {
+            // Put this into else clause so the message is only constructed when necessary.
+            ARMARX_CHECK(false)
+                    << "Expected joint angles to contain '" << currentNode->getName() << ", "
+                    << "but contained: " << simox::alg::get_keys(currentJointAngles);
+            return;
+        }
+
         float pos = currentNode->getJointValue() * conversionFactor;
-        ARMARX_INFO << "setting position control for current Node Name: " << currentNode->getName() << " with current value: " << pos;
+        ARMARX_INFO << "Setting position control for current node "
+                    << "(name '" << currentNode->getName() << "' with current value " << pos << ")";
 
         // Setting the slider position to pos will set the position to the slider tick closest to pos
         // This will initially send a position target with a small delta to the joint.