From a4d58fc766890a4a8477bb0bf7397f6cc1a1c0f3 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 27 May 2021 18:25:58 +0200
Subject: [PATCH] RobotNameHelper with additional functionality to load
RobotInfoNode from xml file
---
.../RobotNameHelper.cpp | 91 ++++++++++++-------
.../RobotStatechartHelpers/RobotNameHelper.h | 8 ++
2 files changed, 68 insertions(+), 31 deletions(-)
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
index 6753e0a6c..d256ee202 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
@@ -23,24 +23,23 @@
#include "RobotNameHelper.h"
-#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/system/ArmarXDataPath.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/statechart/xmlstates/profiles/StatechartProfiles.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
// #include <ArmarXCore/core/system/ArmarXDataPath.cpp>
-
#include <Eigen/Dense>
+#include <algorithm>
namespace armarx
{
- void RobotNameHelper::writeRobotInfoNode(
- const RobotInfoNodePtr& n,
- std::ostream& str,
- std::size_t indent,
- char indentChar)
+ void RobotNameHelper::writeRobotInfoNode(const RobotInfoNodePtr& n,
+ std::ostream& str,
+ std::size_t indent,
+ char indentChar)
{
const std::string ind(4 * indent, indentChar);
if (!n)
@@ -59,11 +58,12 @@ namespace armarx
return robotInfo;
}
- const std::string RobotNameHelper::LocationLeft = "Left";
+ const std::string RobotNameHelper::LocationLeft = "Left";
const std::string RobotNameHelper::LocationRight = "Right";
- RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
- : root(Node(robotInfo)), robotInfo(robotInfo)
+ RobotNameHelper::RobotNameHelper(const RobotInfoNodePtr& robotInfo,
+ const StatechartProfilePtr& profile) :
+ root(Node(robotInfo)), robotInfo(robotInfo)
{
ARMARX_TRACE;
StatechartProfilePtr p = profile;
@@ -73,7 +73,6 @@ namespace armarx
p = p->getParent();
}
profiles.emplace_back(""); // for matching the default/root
-
}
std::string RobotNameHelper::select(const std::string& path) const
@@ -94,17 +93,50 @@ namespace armarx
return node.value();
}
- RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile)
+ RobotNameHelperPtr RobotNameHelper::Create(const RobotInfoNodePtr& robotInfo,
+ const StatechartProfilePtr& profile)
{
return RobotNameHelperPtr(new RobotNameHelper(robotInfo, profile));
}
+ RobotNameHelperPtr RobotNameHelper::Create(const std::string& robotXmlFilename,
+ const StatechartProfilePtr& profile)
+ {
+ RapidXmlReaderPtr reader = RapidXmlReader::FromFile(robotXmlFilename);
+ RapidXmlReaderNode robotNode = reader->getRoot("Robot");
+
+ return Create(readRobotInfo(robotNode.first_node("RobotInfo")), profile);
+ }
+
+ RobotInfoNodePtr RobotNameHelper::readRobotInfo(const RapidXmlReaderNode& infoNode)
+ {
+ const std::string name = infoNode.name();
+ const std::string profile = infoNode.attribute_value_or_default("profile", "");
+ const std::string value = infoNode.attribute_value_or_default("value", "");
+
+ const auto nodes = infoNode.nodes();
+
+ std::vector<RobotInfoNodePtr> children;
+ children.reserve(nodes.size());
+
+ std::transform(nodes.begin(),
+ nodes.end(),
+ std::back_inserter(children),
+ [](const auto & childNode)
+ {
+ return readRobotInfo(childNode);
+ });
+
+ return new RobotInfoNode(name, profile, value, children);
+ }
+
RobotNameHelper::Arm RobotNameHelper::getArm(const std::string& side) const
{
return Arm(shared_from_this(), side);
}
- RobotNameHelper::RobotArm RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const
+ RobotNameHelper::RobotArm
+ RobotNameHelper::getRobotArm(const std::string& side, const VirtualRobot::RobotPtr& robot) const
{
return RobotArm(Arm(shared_from_this(), side), robot);
}
@@ -114,11 +146,7 @@ namespace armarx
return rnh;
}
- RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo)
- : robotInfo(robotInfo)
- {
-
- }
+ RobotNameHelper::Node::Node(const RobotInfoNodePtr& robotInfo) : robotInfo(robotInfo) {}
std::string RobotNameHelper::Node::value()
{
@@ -126,7 +154,8 @@ namespace armarx
return robotInfo->value;
}
- RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name, const std::vector<std::string>& profiles)
+ RobotNameHelper::Node RobotNameHelper::Node::select(const std::string& name,
+ const std::vector<std::string>& profiles)
{
ARMARX_TRACE;
if (!isValid())
@@ -168,7 +197,6 @@ namespace armarx
}
}
-
std::string RobotNameHelper::Arm::getSide() const
{
return side;
@@ -232,8 +260,7 @@ namespace armarx
{
ArmarXDataPath::FindPackageAndAddDataPath(getHandModelPackage());
auto path = getHandModelPath();
- return ArmarXDataPath::getAbsolutePath(path, path) ?
- path : "";
+ return ArmarXDataPath::getAbsolutePath(path, path) ? path : "";
}
std::string RobotNameHelper::Arm::getHandModelPackage() const
@@ -254,16 +281,17 @@ namespace armarx
return select("FullHandCollisionModel");
}
- RobotNameHelper::RobotArm RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
+ RobotNameHelper::RobotArm
+ RobotNameHelper::Arm::addRobot(const VirtualRobot::RobotPtr& robot) const
{
ARMARX_TRACE;
return RobotArm(*this, robot);
}
- RobotNameHelper::Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh, const std::string& side)
- : rnh(rnh), side(side)
+ RobotNameHelper::Arm::Arm(const std::shared_ptr<const RobotNameHelper>& rnh,
+ const std::string& side) :
+ rnh(rnh), side(side)
{
-
}
std::string RobotNameHelper::Arm::select(const std::string& path) const
@@ -310,8 +338,9 @@ namespace armarx
{
ARMARX_TRACE;
std::string abs;
- ARMARX_CHECK_EXPRESSION(ArmarXDataPath::SearchReadableFile(arm.getFullHandCollisionModel(), abs));
- return VirtualRobot::TriMeshModel::FromFile(abs);
+ ARMARX_CHECK_EXPRESSION(
+ ArmarXDataPath::SearchReadableFile(arm.getFullHandCollisionModel(), abs));
+ return VirtualRobot::TriMeshModel::FromFile(abs);
}
VirtualRobot::RobotNodePtr RobotNameHelper::RobotArm::getHandRootNode() const
@@ -341,8 +370,8 @@ namespace armarx
return arm;
}
- RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot)
- : arm(arm), robot(robot)
+ RobotNameHelper::RobotArm::RobotArm(const Arm& arm, const VirtualRobot::RobotPtr& robot) :
+ arm(arm), robot(robot)
{
ARMARX_CHECK_NOT_NULL(robot);
}
@@ -351,4 +380,4 @@ namespace armarx
{
return profiles;
}
-}
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
index d37ecea9d..65fbc12c2 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
@@ -26,6 +26,7 @@
#include <RobotAPI/interface/core/RobotState.h>
#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
namespace armarx
@@ -33,6 +34,8 @@ namespace armarx
using RobotNameHelperPtr = std::shared_ptr<class RobotNameHelper>;
using StatechartProfilePtr = std::shared_ptr<class StatechartProfile>;
+ class RapidXmlReaderNode;
+
class RobotNameHelper : public std::enable_shared_from_this<RobotNameHelper>
{
public:
@@ -128,8 +131,10 @@ namespace armarx
std::string select(const std::string& path) const;
static RobotNameHelperPtr Create(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile);
+ static RobotNameHelperPtr Create(const std::string& robotXmlFilename, const StatechartProfilePtr& profile = nullptr);
RobotNameHelper(const RobotInfoNodePtr& robotInfo, const StatechartProfilePtr& profile = nullptr);
+
RobotNameHelper(RobotNameHelper&&) = default;
RobotNameHelper(const RobotNameHelper&) = default;
RobotNameHelper& operator=(RobotNameHelper&&) = default;
@@ -140,6 +145,9 @@ namespace armarx
const std::vector<std::string>& getProfiles() const;
const RobotInfoNodePtr& getRobotInfoNodePtr() const;
private:
+
+ static RobotInfoNodePtr readRobotInfo(const RapidXmlReaderNode& infoNode);
+
RobotNameHelper& self()
{
return *this;
--
GitLab