Fixed bug where multiple cartesian selections tried to use the same tcp

controller

source/RobotAPI/components/units/RobotUnit/Units/TCPControllerSubUnit.cpp

@@ -70,23 +70,7 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
         ARMARX_CHECK_EXPRESSION_W_HINT(coordinateTransformationRobot->hasRobotNode(tcpName), "The robot does not have the node: " + tcpName);
         tcp = tcpName;
     }
-    auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName;
-    auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
-    NJointCartesianVelocityControllerWithRampPtr tcpController;
-    bool nodeSetAlreadyControlled = false;
-    for (NJointControllerPtr controller : activeNJointControllers)
-    {
-        tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
-        if (!tcpController)
-        {
-            continue;
-        }
-        if (tcpController->getNodeSetName() == nodeSetName && tcpController->getInstanceName() == controllerName)
-        {
-            nodeSetAlreadyControlled = true;
-            break;
-        }
-    }
+
     robotUnit->updateVirtualRobot(coordinateTransformationRobot);
 
     float xVel = 0.f;
@@ -135,7 +119,23 @@ void TCPControllerSubUnit::setTCPVelocity(const std::string& nodeSetName, const
         noMode = true;
     }
     ARMARX_DEBUG << "CartesianSelection-Mode: " << (int)mode;
-
+    auto controllerName = this->getName() + "_" + tcp + "_" + nodeSetName + "_mode_" + std::to_string((int)mode);
+    auto activeNJointControllers = robotUnit->getNJointControllersNotNull(robotUnit->getNJointControllerNames());
+    NJointCartesianVelocityControllerWithRampPtr tcpController;
+    bool nodeSetAlreadyControlled = false;
+    for (NJointControllerPtr controller : activeNJointControllers)
+    {
+        tcpController = NJointCartesianVelocityControllerWithRampPtr::dynamicCast(controller);
+        if (!tcpController)
+        {
+            continue;
+        }
+        if (tcpController->getNodeSetName() == nodeSetName && tcpController->getInstanceName() == controllerName)
+        {
+            nodeSetAlreadyControlled = true;
+            break;
+        }
+    }
 
     if (!nodeSetAlreadyControlled)
     {