Improve RobotNameHelper

* Add getRobotInfoNodePtr
* trace code
* make exception output more useful

source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp

@@ -24,18 +24,45 @@
 #include "RobotNameHelper.h"
 
 #include <ArmarXCore/core/util/StringHelpers.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 #include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
+#include <ArmarXCore/util/CPPUtility/trace.h>
 
 #include <Eigen/Dense>
 
 namespace armarx
 {
+
+    void RobotNameHelper::writeRobotInfoNode(
+        const RobotInfoNodePtr& n,
+        std::ostream& str,
+        std::size_t indent,
+        char indentChar)
+    {
+        const std::string ind(4 * indent, indentChar);
+        if (!n)
+        {
+            str << ind << "nullptr\n";
+            return;
+        }
+        str << ind << n->name << ", profile = " << n->profile << ", value " << n->value << '\n';
+        for (const auto c : n->children)
+        {
+            writeRobotInfoNode(c, str, indent + 1);
+        }
+    }
+    const RobotInfoNodePtr& RobotNameHelper::getRobotInfoNodePtr() const
+    {
+        return robotInfo;
+    }
+
     const std::string RobotNameHelper::LocationLeft = "Left";
     const std::string RobotNameHelper::LocationRight = "Right";
 
     RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
-        : root(Node(robotInfo))
+        : root(Node(robotInfo)), robotInfo(robotInfo)
     {
+        ARMARX_TRACE;
         StatechartProfilePtr p = profile;
         while (p && !p->isRoot())
         {
@@ -48,6 +75,7 @@ namespace armarx
 
     std::string RobotNameHelper::select(const std::string& path) const
     {
+        ARMARX_TRACE;
         Node node = root;
         for (const std::string& p : Split(path, "/"))
         {
@@ -55,7 +83,10 @@ namespace armarx
         }
         if (!node.isValid())
         {
-            throw std::runtime_error("RobotNameHelper::select: path " + path + " not found");
+            std::stringstream s;
+            s << "RobotNameHelper::select: path " + path + " not found\nrobotInfo:\n";
+            writeRobotInfoNode(robotInfo, s);
+            throw std::runtime_error(s.str());
         }
         return node.value();
     }
@@ -94,6 +125,7 @@ namespace armarx
 
     RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name, const std::vector<std::string>& profiles)
     {
+        ARMARX_TRACE;
         if (!isValid())
         {
             return Node(nullptr);
@@ -125,7 +157,11 @@ namespace armarx
     {
         if (!isValid())
         {
-            throw std::runtime_error("RobotNameHelper::Node nullptr exception");
+
+            std::stringstream s;
+            s << "RobotNameHelper::Node nullptr exception\nrobotInfo:\n";
+            writeRobotInfoNode(robotInfo, s);
+            throw std::runtime_error(s.str());
         }
     }
 
@@ -137,56 +173,67 @@ namespace armarx
 
     std::string RobotNameHelper::Arm::getKinematicChain() const
     {
+        ARMARX_TRACE;
         return select("KinematicChain");
     }
 
     std::string RobotNameHelper::Arm::getTorsoKinematicChain() const
     {
+        ARMARX_TRACE;
         return select("TorsoKinematicChain");
     }
 
     std::string RobotNameHelper::Arm::getTCP() const
     {
+        ARMARX_TRACE;
         return select("TCP");
     }
 
     std::string RobotNameHelper::Arm::getForceTorqueSensor() const
     {
+        ARMARX_TRACE;
         return select("ForceTorqueSensor");
     }
 
     std::string RobotNameHelper::Arm::getEndEffector() const
     {
+        ARMARX_TRACE;
         return select("EndEffector");
     }
 
     std::string RobotNameHelper::Arm::getMemoryHandName() const
     {
+        ARMARX_TRACE;
         return select("MemoryHandName");
     }
 
     std::string RobotNameHelper::Arm::getHandControllerName() const
     {
+        ARMARX_TRACE;
         return select("HandControllerName");
     }
 
     std::string RobotNameHelper::Arm::getHandRootNode() const
     {
+        ARMARX_TRACE;
         return select("HandRootNode");
     }
 
     std::string RobotNameHelper::Arm::getHandModelPath() const
     {
+        ARMARX_TRACE;
         return select("HandModelPath");
     }
 
     std::string RobotNameHelper::Arm::getHandModelPackage() const
     {
+        ARMARX_TRACE;
         return select("HandModelPackage");
     }
 
     RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
     {
+        ARMARX_TRACE;
         return RobotArm(*this, robot);
     }
 
@@ -198,37 +245,48 @@ namespace armarx
 
     std::string RobotNameHelper::Arm::select(const std::string& path) const
     {
+        ARMARX_TRACE;
         return rnh->select(side + "Arm/" + path);
     }
 
     std::string RobotNameHelper::RobotArm::getSide() const
     {
+        ARMARX_TRACE;
         return arm.getSide();
     }
 
     VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getKinematicChain() const
     {
+        ARMARX_TRACE;
         return robot->getRobotNodeSet(arm.getKinematicChain());
     }
 
     VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getTorsoKinematicChain() const
     {
+        ARMARX_TRACE;
         return robot->getRobotNodeSet(arm.getTorsoKinematicChain());
     }
 
     VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getTCP() const
     {
+        ARMARX_TRACE;
         return robot->getRobotNode(arm.getTCP());
     }
 
     VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getHandRootNode() const
     {
+        ARMARX_TRACE;
         return robot->getRobotNode(arm.getHandRootNode());
     }
 
     Eigen::Matrix4f RobotNameHelper::RobotArm::getTcp2HandRootTransform() const
     {
-        return getTCP()->getPoseInRootFrame().inverse() * getHandRootNode()->getPoseInRootFrame();
+        ARMARX_TRACE;
+        const auto tcp = getTCP();
+        ARMARX_CHECK_NOT_NULL(tcp);
+        const auto hand = getHandRootNode();
+        ARMARX_CHECK_NOT_NULL(hand);
+        return tcp->getPoseInRootFrame().inverse() * hand->getPoseInRootFrame();
     }
 
     RobotNameHelper::Arm RobotNameHelper::RobotArm::getArm() const

source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h

@@ -35,6 +35,12 @@ namespace armarx
     class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
     {
     public:
+        static void writeRobotInfoNode(
+            const RobotInfoNodePtr& n,
+            std::ostream& str,
+            std::size_t indent = 0,
+            char indentChar = ' ');
+
         class Node
         {
         public:
@@ -79,8 +85,9 @@ namespace armarx
             Arm& operator=(const Arm&) = default;
 
             std::shared_ptr<const RobotNameHelper> getRobotNameHelper() const;
-        private:
+
             std::string select(const std::string& path) const;
+        private:
 
             std::shared_ptr<const RobotNameHelper> rnh;
             std::string side;
@@ -120,6 +127,7 @@ namespace armarx
         Arm getArm(const std::string& side) const;
         RobotArm getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const;
         const std::vector<std::string>& getProfiles() const;
+        const RobotInfoNodePtr& getRobotInfoNodePtr() const;
     private:
         RobotNameHelper& self()
         {
@@ -128,7 +136,6 @@ namespace armarx
 
         Node root;
         std::vector<std::string> profiles;
-
-
+        RobotInfoNodePtr robotInfo;
     };
 }