Skip to content
Snippets Groups Projects
Commit a4d58fc7 authored by Fabian Reister's avatar Fabian Reister
Browse files

RobotNameHelper with additional functionality to load RobotInfoNode from xml file

parent f9f16c90
No related branches found
No related tags found
2 merge requests!157armem/dev => master,!154armem_robot_state: updates
This commit is part of merge request !157. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.cpp
+ 60
− 31
Edit View file @ a4d58fc7
......@@ -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
source/RobotAPI/libraries/RobotStatechartHelpers/RobotNameHelper.h
+ 8
− 0
Edit View file @ a4d58fc7
......@@ -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;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment