From 247761b580beff0655d766c6bd0363f88f81fc04 Mon Sep 17 00:00:00 2001
From: Raphael Grimm <raphael.grimm@kit.edu>
Date: Thu, 9 Jul 2020 17:15:48 +0200
Subject: [PATCH] Improve RobotNameHelper
* Add getRobotInfoNodePtr
* trace code
* make exception output more useful
---
.../RobotNameHelper.cpp | 66 +++++++++++++++++--
.../RobotStatechartHelpers/RobotNameHelper.h | 13 +++-
2 files changed, 72 insertions(+), 7 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index dc4346fb8..a9ecec5e4 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/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
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index c255825bd..703c0ae83 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/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;
};
}
--
GitLab