yaml.cpp

Go to the documentation of this file.

#include <se2ez/core/io.h>
// #include <se2ez/core/log.h>
// #include <se2ez/core/math.h>
#include <se2ez/core/yaml.h>
#include <se2ez/core/yaml/utility.h>

#include <se2ez/plan/constraint.h>

using namespace se2ez;

namespace
{
    ompl::base::ConstraintPtr YAMLToPointConstraint(const RobotPtr &robot, const YAML::Node &node)
    {
        if (!io::isNode(node["name"]))
            io::throwParsingError(node, "\"name\" entry in YAML does not exist");

        if (!io::isNode(node["point"]))
            io::throwParsingError(node, "\"point\" entry in YAML does not exist");

        if (!io::isNode(node["ee"]))
            io::throwParsingError(node, "\"ee\" entry in YAML does not exist");

        std::string base = "root";
        if (io::isNode(node["base"]))
            base = node["base"].as<std::string>();

        return std::make_shared<plan::PointConstraint>(node["name"].as<std::string>(), robot,
                                                       node["ee"].as<std::string>(), base,
                                                       io::toVector(node["point"]));
    }

    ompl::base::ConstraintPtr YAMLToLineConstraint(const RobotPtr &robot, const YAML::Node &node)
    {
        if (!io::isNode(node["name"]))
            io::throwParsingError(node, "\"name\" entry in YAML does not exist");

        if (!io::isNode(node["a"]))
            io::throwParsingError(node, "\"a\" entry in YAML does not exist");

        if (!io::isNode(node["b"]))
            io::throwParsingError(node, "\"b\" entry in YAML does not exist");

        if (!io::isNode(node["ee"]))
            io::throwParsingError(node, "\"ee\" entry in YAML does not exist");

        std::string base = "root";
        if (io::isNode(node["base"]))
            base = node["base"].as<std::string>();

        auto constraint = std::make_shared<plan::LineConstraint>(
            node["name"].as<std::string>(), robot, node["ee"].as<std::string>(), base,
            io::toVector(node["a"]), io::toVector(node["b"]));

        if (io::isNode(node["draw"]))
            constraint->setDrawOffset(io::toVector(node["draw"]));

        return constraint;
    }

    ompl::base::ConstraintPtr YAMLToCircleConstraint(const RobotPtr &robot, const YAML::Node &node)
    {
        if (!io::isNode(node["name"]))
            io::throwParsingError(node, "\"name\" entry in YAML does not exist");

        if (!io::isNode(node["p"]))
            io::throwParsingError(node, "\"a\" entry in YAML does not exist");

        if (!io::isNode(node["r"]))
            io::throwParsingError(node, "\"b\" entry in YAML does not exist");

        if (!io::isNode(node["ee"]))
            io::throwParsingError(node, "\"ee\" entry in YAML does not exist");

        unsigned int exp = 2;
        if (io::isNode(node["exp"]))
            exp = node["exp"].as<unsigned int>();

        std::string base = "root";
        if (io::isNode(node["base"]))
            base = node["base"].as<std::string>();

        auto constraint = std::make_shared<plan::CircleConstraint>(
            node["name"].as<std::string>(), robot, node["ee"].as<std::string>(), base,
            io::toVector(node["p"]), io::toVector(node["r"]), exp);

        if (io::isNode(node["draw"]))
            constraint->setDrawOffset(io::toVector(node["draw"]));

        return constraint;
    }



    void YAMLToConstraint(const RobotPtr &robot, std::map<std::string, ompl::base::ConstraintPtr> &map,
                          const YAML::Node &node)
    {
        if (!io::isNode(node["type"]))
            io::throwParsingError(node, "\"type\" entry in YAML does not exist");

        std::string type = node["type"].as<std::string>();

        if (!io::isNode(node["name"]))
            io::throwParsingError(node, "\"name\" entry in YAML does not exist");

        std::string name = node["name"].as<std::string>();

        if (type == "line")
            map.emplace(name, YAMLToLineConstraint(robot, node));

        else if (type == "point")
            map.emplace(name, YAMLToPointConstraint(robot, node));

        else if (type == "circle")
            map.emplace(name, YAMLToCircleConstraint(robot, node));

        else
            io::throwParsingError(node["type"], "\"type\" entry in YAML is not valid!");
    }

    void YAMLToConstraints(const RobotPtr &robot, std::map<std::string, ompl::base::ConstraintPtr> &map,
                           const YAML::Node &node)
    {
        if (!io::isNode(node["constraints"]))
            return;

        if (!node["constraints"].IsSequence())
            io::throwParsingError(node["constraints"], "\"constraints\" entry in YAML not a sequence");

        for (const auto &constraint : node["constraints"])
            YAMLToConstraint(robot, map, constraint);
    }
}  // namespace

std::map<std::string, ompl::base::ConstraintPtr> plan::loadConstraints(const RobotPtr &robot,
                                                                       const std::string &filename)
{
    std::map<std::string, ompl::base::ConstraintPtr> map;
    const auto &result = io::loadFileToYAML(filename);

    if (result.first)
    {
        try
        {
            YAMLToConstraints(robot, map, result.second);
        }
        catch (std::exception &e)
        {
            SE2EZ_ERROR("In YAML File %1%: %2%", io::resolvePath(filename), e.what());
            map.clear();
        }
    }
    else
        SE2EZ_ERROR("Failed to load file \"%1%\"", filename);

    return map;
}