armem/dev => master

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