From d982bcfd9709efeaa05a52c8cb3cb6539f2829c3 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Fri, 10 Aug 2018 11:50:16 +0200
Subject: [PATCH] added RobotNameHelper::RobotArm

---
 .../RobotNameHelper.cpp                       | 39 +++++++++++++++++--
 .../RobotStatechartHelpers/RobotNameHelper.h  | 25 ++++++++++--
 2 files changed, 57 insertions(+), 7 deletions(-)

diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 88eeff34e..5bf8b9eaa 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -63,8 +63,12 @@ RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo, co
 
 RobotNameHelper::Arm RobotNameHelper::getArm(const std::string& side)
 {
-    std::weak_ptr<RobotNameHelper> rnh = shared_from_this();
-    return Arm(rnh, side);
+    return Arm(shared_from_this(), side);
+}
+
+RobotNameHelper::RobotArm RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot)
+{
+    return RobotArm(Arm(shared_from_this(), side), robot);
 }
 
 RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo)
@@ -152,7 +156,12 @@ std::string RobotNameHelper::Arm::getHandControllerName() const
     return select("HandControllerName");
 }
 
-RobotNameHelper::Arm::Arm(const std::weak_ptr<RobotNameHelper>& rnh, const std::string& side)
+RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
+{
+    return RobotArm(*this, robot);
+}
+
+RobotNameHelper::Arm::Arm(const std::shared_ptr<RobotNameHelper>& rnh, const std::string& side)
     : rnh(rnh), side(side)
 {
 
@@ -160,5 +169,27 @@ RobotNameHelper::Arm::Arm(const std::weak_ptr<RobotNameHelper>& rnh, const std::
 
 std::string RobotNameHelper::Arm::select(const std::string& path) const
 {
-    return rnh.lock()->select(side + "Arm/" + path);
+    return rnh->select(side + "Arm/" + path);
+}
+
+VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getKinematicChain() const
+{
+    return robot->getRobotNodeSet(arm.getKinematicChain());
+}
+
+VirtualRobot::RobotNodeSetPtr RobotNameHelper::RobotArm::getTorsoKinematicChain() const
+{
+    return robot->getRobotNodeSet(arm.getTorsoKinematicChain());
+}
+
+VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getTCP() const
+{
+    return robot->getRobotNode(arm.getTCP());
 }
+
+RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot)
+    : arm(arm), robot(robot)
+{
+
+}
+
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 1d0dc2924..0f633f431 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -29,6 +29,7 @@
 
 #include <boost/optional.hpp>
 
+#include <VirtualRobot/Robot.h>
 
 namespace armarx
 {
@@ -54,6 +55,7 @@ namespace armarx
         static const std::string LocationLeft;
         static const std::string LocationRight;
 
+        struct RobotArm;
         struct Arm
         {
             friend class RobotNameHelper;
@@ -66,21 +68,38 @@ namespace armarx
             std::string getEndEffector() const;
             std::string getMemoryHandName() const;
             std::string getHandControllerName() const;
-
+            RobotArm addRobot(const VirtualRobot::RobotPtr& robot) const;
 
         private:
-            Arm(const std::weak_ptr<RobotNameHelper>& rnh, const std::string& side);
+            Arm(const std::shared_ptr<RobotNameHelper>& rnh, const std::string& side);
             std::string select(const std::string& path) const;
 
-            std::weak_ptr<RobotNameHelper> rnh;
+            std::shared_ptr<RobotNameHelper> rnh;
             std::string side;
         };
 
+        struct RobotArm
+        {
+            friend class RobotNameHelper;
+            friend class Arm;
+        public:
+            VirtualRobot::RobotNodeSetPtr getKinematicChain() const;
+            VirtualRobot::RobotNodeSetPtr getTorsoKinematicChain() const;
+            VirtualRobot::RobotNodePtr getTCP() const;
+
+        private:
+            RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot);
+
+            const Arm arm;
+            VirtualRobot::RobotPtr robot;
+        };
+
         std::string select(const std::string& path) const;
 
         static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
 
         Arm getArm(const std::string& side);
+        RobotArm getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot);
 
     private:
         RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
-- 
GitLab