From 38b834974ce8260820dfe549a0900e186dda8a87 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Fri, 1 Mar 2019 18:34:22 +0100
Subject: [PATCH] KinematicUnitObserver rejects now joint that are not in the
joint set instead of throwing an error
---
.../components/units/KinematicUnitObserver.cpp | 15 +++++++++++----
.../components/units/KinematicUnitObserver.h | 1 +
2 files changed, 12 insertions(+), 4 deletions(-)
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.cpp b/source/RobotAPI/components/units/KinematicUnitObserver.cpp
index bdf8dc94e..3f8f026aa 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.cpp
+++ b/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);
diff --git a/source/RobotAPI/components/units/KinematicUnitObserver.h b/source/RobotAPI/components/units/KinematicUnitObserver.h
index 1f306d686..0bbdbb054 100644
--- a/source/RobotAPI/components/units/KinematicUnitObserver.h
+++ b/source/RobotAPI/components/units/KinematicUnitObserver.h
@@ -155,6 +155,7 @@ namespace armarx
Mutex initializedChannelsMutex;
private:
std::string robotNodeSetName;
+ std::set<std::string> robotNodes;
};
--
GitLab