From 83dd3a3baf5e743ba18e8073bbc25de5ed5eeeb2 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Thu, 9 Aug 2018 18:39:22 +0200
Subject: [PATCH] added Arms to RobotNameHelper
---
data/RobotAPI/robots/Armar3/ArmarIII.xml | 27 ++++---
.../RobotNameHelper.cpp | 72 +++++++++++--------
.../RobotStatechartHelpers/RobotNameHelper.h | 51 +++++++------
.../RobotNameHelperTestGroup/TestGetNames.cpp | 8 ++-
4 files changed, 91 insertions(+), 67 deletions(-)
diff --git a/data/RobotAPI/robots/Armar3/ArmarIII.xml b/data/RobotAPI/robots/Armar3/ArmarIII.xml
index 5a09f5d28..cf51f0c2e 100644
--- a/data/RobotAPI/robots/Armar3/ArmarIII.xml
+++ b/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>
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 138c7ab70..88eeff34e 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -1,6 +1,6 @@
/*
* 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);
}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index 6beaf49b9..1d0dc2924 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -1,6 +1,6 @@
/*
* 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;
};
}
diff --git a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp b/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp
index 594e8b158..75e6d0ee5 100644
--- a/source/RobotAPI/statecharts/RobotNameHelperTestGroup/TestGetNames.cpp
+++ b/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();
}
--
GitLab