KinematicUnitObserver rejects now joint that are not in the joint set instead of throwing an error

source/RobotAPI/components/units/KinematicUnitObserver.cpp

@@ -91,11 +91,12 @@ void KinematicUnitObserver::onConnectObserver()
         throw UserException("RobotNodeSet not defined");
     }
 
-    VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
+    auto robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
 
     std::vector< VirtualRobot::RobotNodePtr > robotNodes;
     robotNodes = robotNodeSetPtr->getAllRobotNodes();
-
+    auto robotNodeNames = robotNodeSetPtr->getNodeNames();
+    this->robotNodes = std::set<std::string>(robotNodeNames.begin(), robotNodeNames.end());
     // register all channels
     offerChannel("jointangles", "Joint values of the " + robotNodeSetName + " kinematic chain");
     offerChannel("jointvelocities", "Joint velocities of the " + robotNodeSetName + " kinematic chain");
@@ -298,14 +299,20 @@ void KinematicUnitObserver::nameValueMapToDataFields(const std::string& channelN
         {
             for (const auto& it : nameValueMap)
             {
-                map[it.first] = new Variant(it.second);
+                if (robotNodes.count(it.first))
+                {
+                    map[it.first] = new Variant(it.second);
+                }
             }
         }
         else
         {
             for (const auto& it : nameValueMap)
             {
-                map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
+                if (robotNodes.count(it.first))
+                {
+                    map[it.first] = new TimedVariant(new Variant(it.second), IceUtil::Time::microSeconds(timestamp));
+                }
             }
         }
         setDataFieldsFlatCopy(channelName, map);

source/RobotAPI/components/units/KinematicUnitObserver.h

@@ -155,6 +155,7 @@ namespace armarx
         Mutex initializedChannelsMutex;
     private:
         std::string robotNodeSetName;
+        std::set<std::string> robotNodes;
     };