added Arms to RobotNameHelper

data/RobotAPI/robots/Armar3/ArmarIII.xml

@@ -495,17 +495,22 @@
     </RobotNodeSet>
 
     <RobotInfo>
-        <KinematicChains>
-            <LeftArm  value="LeftArm"  />
-            <RightArm value="RightArm" />
-            <LeftArmTCP  value="TCP L" />
-            <RightArmTCP value="TCP R" />
-            <TorsoLeftArm  value="HipYawLeftArm"  />
-            <TorsoRightArm value="HipYawRightArm" />
-        </KinematicChains>
-
-        <test value="abc" />
-        <test profile="Armar3Simulation" value="abc_sim" />
+        <LeftArm>
+            <KinematicChain      value="LeftArm" />
+            <TorsoKinematicChain value="HipYawLeftArm" />
+            <TCP value="TCP L" />
+            <ForceTorqueSensor value="FT L" />
+            <EndEffector value="TCP L" />
+            <MemoryHandName value="???" />
+        </LeftArm>
+        <RightArm>
+            <KinematicChain      value="RightArm" />
+            <TorsoKinematicChain value="HipYawRightArm" />
+            <TCP value="TCP R" />
+            <ForceTorqueSensor value="FT R" />
+            <EndEffector value="TCP R" />
+            <MemoryHandName value="???" />
+        </RightArm>
     </RobotInfo>
 
 </Robot>

source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp

 /*
  * This file is part of ArmarX.
- * 
+ *
  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
  * Karlsruhe Institute of Technology (KIT), all rights reserved.
  *
@@ -30,10 +30,10 @@ const std::string RobotNameHelper::LocationLeft = "Left";
 const std::string RobotNameHelper::LocationRight = "Right";
 
 RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
-    : root(Node(robotInfo)), kinematicChains(self())
+    : root(Node(robotInfo))
 {
     StatechartProfilePtr p = profile;
-    while(p && !p->isRoot())
+    while (p && !p->isRoot())
     {
         profiles.emplace_back(p->getName());
         p = p->getParent();
@@ -45,17 +45,28 @@ RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo, const Statec
 std::string RobotNameHelper::select(const std::string& path) const
 {
     Node node = root;
-    for(const std::string& p : Split(path, "/"))
+    for (const std::string& p : Split(path, "/"))
     {
         node = node.select(p, profiles);
     }
-    if(!node.isValid())
+    if (!node.isValid())
     {
         throw std::runtime_error("RobotNameHelper::select: path " + path + " not found");
     }
     return node.value();
 }
 
+RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
+{
+    return RobotNameHelperPtr(new RobotNameHelper(robotInfo, profile));
+}
+
+RobotNameHelper::Arm RobotNameHelper::getArm(const std::string& side)
+{
+    std::weak_ptr<RobotNameHelper> rnh = shared_from_this();
+    return Arm(rnh, side);
+}
+
 RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo)
     : robotInfo(robotInfo)
 {
@@ -70,21 +81,21 @@ std::string RobotNameHelper::Node::value()
 
 RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name, const std::vector<std::string>& profiles)
 {
-    if(!isValid())
+    if (!isValid())
     {
         return Node(nullptr);
     }
     std::map<std::string, RobotInfoNodePtr> matches;
-    for(const RobotInfoNodePtr& child : robotInfo->children)
+    for (const RobotInfoNodePtr& child : robotInfo->children)
     {
-        if(child->name == name)
+        if (child->name == name)
         {
             matches[child->profile] = child;
         }
     }
-    for(const std::string& p : profiles)
+    for (const std::string& p : profiles)
     {
-        if(matches.count(p))
+        if (matches.count(p))
         {
             return matches.at(p);
         }
@@ -99,54 +110,55 @@ bool RobotNameHelper::Node::isValid()
 
 void RobotNameHelper::Node::checkValid()
 {
-    if(!isValid())
+    if (!isValid())
     {
         throw std::runtime_error("RobotNameHelper::Node nullptr exception");
     }
 }
 
-RobotNameHelper::KinematicChains::KinematicChains(const RobotNameHelper& rnh)
-    : rnh(rnh)
-{
 
+std::string RobotNameHelper::Arm::getKinematicChain() const
+{
+    return select("KinematicChain");
 }
 
-std::string RobotNameHelper::KinematicChains::getArm(const std::string& location) const
+std::string RobotNameHelper::Arm::getTorsoKinematicChain() const
 {
-    return rnh.select("KinematicChains/" + location + "Arm");
+    return select("TorsoKinematicChain");
 }
 
-std::string RobotNameHelper::KinematicChains::getLeftArm() const
+std::string RobotNameHelper::Arm::getTCP() const
 {
-    return getArm(LocationLeft);
+    return select("TCP");
 }
 
-std::string RobotNameHelper::KinematicChains::getRightArm() const
+std::string RobotNameHelper::Arm::getForceTorqueSensor() const
 {
-    return getArm(LocationRight);
+    return select("ForceTorqueSensor");
 }
 
-std::string RobotNameHelper::KinematicChains::getTorsoLeftArm() const
+std::string RobotNameHelper::Arm::getEndEffector() const
 {
-    return getArm("Torso" + LocationLeft);
+    return select("EndEffector");
 }
 
-std::string RobotNameHelper::KinematicChains::getTorsoRightArm() const
+std::string RobotNameHelper::Arm::getMemoryHandName() const
 {
-    return getArm("Torso" + LocationRight);
+    return select("MemoryHandName");
 }
 
-std::string RobotNameHelper::KinematicChains::getArmTCP(const std::string& location) const
+std::string RobotNameHelper::Arm::getHandControllerName() const
 {
-    return rnh.select("KinematicChains/" + location + "ArmTCP");
+    return select("HandControllerName");
 }
 
-std::string RobotNameHelper::KinematicChains::getLeftArmTCP() const
+RobotNameHelper::Arm::Arm(const std::weak_ptr<RobotNameHelper>& rnh, const std::string& side)
+    : rnh(rnh), side(side)
 {
-    return getArmTCP(LocationLeft);
+
 }
 
-std::string RobotNameHelper::KinematicChains::getRightArmTCP() const
+std::string RobotNameHelper::Arm::select(const std::string& path) const
 {
-    return getArmTCP(LocationRight);
+    return rnh.lock()->select(side + "Arm/" + path);
 }

source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h

 /*
  * This file is part of ArmarX.
- * 
+ *
  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
  * Karlsruhe Institute of Technology (KIT), all rights reserved.
  *
@@ -23,17 +23,18 @@
 
 #pragma once
 
-#include <boost/shared_ptr.hpp>
-
 #include <RobotAPI/interface/core/RobotState.h>
 
 #include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
 
+#include <boost/optional.hpp>
+
+
 namespace armarx
 {
-    typedef boost::shared_ptr<class RobotNameHelper> RobotNameHelperPtr;
+    typedef std::shared_ptr<class RobotNameHelper> RobotNameHelperPtr;
 
-    class RobotNameHelper
+    class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
     {
     public:
         class Node
@@ -53,40 +54,44 @@ namespace armarx
         static const std::string LocationLeft;
         static const std::string LocationRight;
 
-        class KinematicChains
+        struct Arm
         {
+            friend class RobotNameHelper;
         public:
-            KinematicChains(const RobotNameHelper& rnh);
-            KinematicChains(KinematicChains const&) = delete;
-            KinematicChains& operator=(KinematicChains const&) = delete;
-
-            std::string getArm(const std::string& location) const;
-            std::string getLeftArm() const;
-            std::string getRightArm() const;
-            std::string getTorsoLeftArm() const;
-            std::string getTorsoRightArm() const;
 
-            std::string getArmTCP(const std::string& location) const;
-            std::string getLeftArmTCP() const;
-            std::string getRightArmTCP() const;
+            std::string getKinematicChain() const;
+            std::string getTorsoKinematicChain() const;
+            std::string getTCP() const;
+            std::string getForceTorqueSensor() const;
+            std::string getEndEffector() const;
+            std::string getMemoryHandName() const;
+            std::string getHandControllerName() const;
 
 
         private:
-            const RobotNameHelper& rnh;
+            Arm(const std::weak_ptr<RobotNameHelper>& rnh, const std::string& side);
+            std::string select(const std::string& path) const;
+
+            std::weak_ptr<RobotNameHelper> rnh;
+            std::string side;
         };
 
-        RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
         std::string select(const std::string& path) const;
 
+        static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
+
+        Arm getArm(const std::string& side);
 
     private:
-        RobotNameHelper& self() { return *this; }
+        RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
+        RobotNameHelper& self()
+        {
+            return *this;
+        }
 
         Node root;
         std::vector<std::string> profiles;
 
-    public:
-        KinematicChains kinematicChains;
 
     };
 }

source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp

@@ -43,9 +43,11 @@ void TestGetNames::onEnter()
 
 void TestGetNames::run()
 {
-    RobotNameHelper rnh(getRobotStateComponent()->getRobotInfo(), getSelectedProfile());
-    ARMARX_IMPORTANT << "LeftArm: " << rnh.kinematicChains.getLeftArm();
-    ARMARX_IMPORTANT << "LeftArm TCP: " << rnh.kinematicChains.getLeftArmTCP();
+    RobotNameHelperPtr rnh = RobotNameHelper::Create(getRobotStateComponent()->getRobotInfo(), getSelectedProfile());
+    RobotNameHelper::Arm arm = rnh->getArm("Left");
+    ARMARX_IMPORTANT << "LeftArm: " << arm.getKinematicChain();
+    ARMARX_IMPORTANT << "LeftArm TCP: " << arm.getTCP();
+    ARMARX_IMPORTANT << "HandControllerName: " << arm.getHandControllerName();
 
 
 }