yaml.cpp

Go to the documentation of this file.

/* Author: Zachary Kingston */

#include <se2ez/core/io.h>
#include <se2ez/core/math.h>
#include <se2ez/core/yaml.h>
#include <se2ez/core/geometry.h>
#include <se2ez/core/frame.h>
#include <se2ez/core/state.h>
#include <se2ez/core/robot.h>
#include <se2ez/core/collision.h>
#include <se2ez/core/yaml/utility.h>

#include <yaml-cpp/yaml.h>

using namespace se2ez;

namespace
{
    /** \brief Parses a YAML node into a geometry object.
     *  \param[in] node Node to parse.
     *  \return A geometry object from the node.
     */
    GeometryPtr YAMLToGeometry(const YAML::Node &node)
    {
        // Extract type
        if (!io::isNode(node["type"]))
            io::throwParsingError(node, "No \"type\" entry in geometry in YAML");

        Geometry::Type type;
        try
        {
            type = Geometry::stringToType(node["type"].as<std::string>());
        }
        catch (...)
        {
            io::throwParsingError(node["type"], "\"type\" is not a valid joint type.");
        }

        // Extract offset
        Eigen::Vector3d offset{0., 0., 0.};
        if (io::isNode(node["offset"]))
        {
            if (node["offset"].size() != 3)
                io::throwParsingError(node["offset"], "\"offset\" must be three-dimensional");

            offset = io::toVector(node["offset"]);
        }

        // Extract color
        Eigen::Vector4d color{0., 0., 0., 1.};
        if (io::isNode(node["color"]))
        {
            if (node["color"].size() != 4)
                io::throwParsingError(node["color"], "\"color\" must be four-dimensional");

            color = io::toVector(node["color"]);
        }

        // Extract dimensions / or points
        if (type == Geometry::BOX || type == Geometry::CIRCLE)
        {
            Eigen::VectorXd dimensions{};
            if (!io::isNode(node["dimensions"]))
                io::throwParsingError(node, "No \"dimensions\" entry in geometry in YAML");

            if (node["dimensions"].IsSequence())
                dimensions = io::toVector(node["dimensions"]);
            else
                dimensions = math::toVector(node["dimensions"].as<double>());

            return std::make_shared<Geometry>(type, dimensions, offset, color);
        }

        if (type == Geometry::CONVEX || type == Geometry::SIMPLE)
        {
            if (!io::isNode(node["points"]))
                io::throwParsingError(node, "No \"points\" entry in geometry in YAML");

            if (!node["points"].IsSequence())
                io::throwParsingError(node["points"], "Points must be a sequence");

            tf::EigenVector<Eigen::Vector2d> points(node["points"].size());
            for (unsigned int i = 0; i < node["points"].size(); ++i)
                points[i] = io::toVector(node["points"][i]);

            return std::make_shared<Geometry>(type, points, offset, color);
        }

        io::throwParsingError(node["type"], "Unhandled geometry type in YAML!");
    }

    /** \brief Parses a YAML node into frame information.
     *  \param[in] node Node to parse.
     *  \return A tuple of the parent, the created frame, and a list of names of frames that are allowable
     * collisions.
     */
    std::tuple<std::string, FramePtr, std::vector<std::string>> YAMLToFrame(const YAML::Node &node)
    {
        // Extract name
        if (!io::isNode(node["name"]))
            io::throwParsingError(node, "No \"name\" entry in frame in YAML");

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

        // Extract joint
        Joint::Type type = Joint::FIXED;
        if (io::isNode(node["joint"]))
        {
            try
            {
                type = Joint::stringToType(node["joint"].as<std::string>());
            }
            catch (...)
            {
                io::throwParsingError(node["joint"], "Invalid joint type!");
            }
        }

        // Extract tip frame
        Eigen::Vector3d tip{0., 0., 0.};
        if (io::isNode(node["tip"]))
        {
            if (node["tip"].size() != 3)
                io::throwParsingError(node["tip"], "\"tip\" must be three-dimensional");

            tip = io::toVector(node["tip"]);
        }

        // Extract limits
        Eigen::VectorXd lower{};
        Eigen::VectorXd upper{};
        if (io::isNode(node["limits"]))
        {
            if (!io::isNode(node["limits"]["upper"]) || !io::isNode(node["limits"]["lower"]))
                io::throwParsingError(node["limits"], "No \"upper\" or \"lower\" entry in frame in YAML");

            if (node["limits"]["upper"].IsSequence())
            {
                upper = io::toVector(node["limits"]["upper"]);
                lower = io::toVector(node["limits"]["lower"]);
            }
            else
            {
                upper = math::toVector(node["limits"]["upper"].as<double>());
                lower = math::toVector(node["limits"]["lower"].as<double>());
            }
        }

        // Extract geometry
        std::vector<GeometryPtr> geometry;
        if (io::isNode(node["geometry"]))
        {
            if (node["geometry"].IsSequence())
                for (const auto &geo : node["geometry"])
                    geometry.emplace_back(YAMLToGeometry(geo));

            else
                io::throwParsingError(node["geometry"], "\"geometry\" must be a sequence");
        }

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

        std::vector<std::string> allowed;
        if (io::isNode(node["allowed"]))
        {
            if (!node["allowed"].IsSequence())
                io::throwParsingError(node["allowed"], "\"allowed\" entry in YAML not a sequence");

            for (const auto &allow : node["allowed"])
                allowed.emplace_back(allow.as<std::string>());
        }

        return std::make_tuple(parent, std::make_shared<Frame>(name, tip, type, lower, upper, geometry),
                               allowed);
    }

    /** \brief Parses a YAML node into a Robot.
     *  \param[in] node Node to parse.
     *  \return An allocated robot.
     */
    RobotPtr YAMLToRobot(const YAML::Node &node)
    {
        RobotPtr robot = std::make_shared<Robot>();

        if (!io::isNode(node["robot"]))
            io::throwParsingError(node, "No \"robot\" entry in YAML");

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

        for (const auto &frame : node["robot"])
        {
            const auto &result = YAMLToFrame(frame);
            robot->attachFrame(std::get<1>(result), std::get<0>(result));

            for (const auto &allow : std::get<2>(result))
                robot->getACM()->disable(allow, std::get<1>(result)->getName());
        }

        robot->compileTree();
        return robot;
    }

    /** \brief Parses a YAML node into a named state for a robot.
     *  \param[in,out] robot Robot to add state to.
     *  \param[in] node Node to parse.
     */
    void YAMLToNamedState(RobotPtr robot, const YAML::Node &node)
    {
        if (!io::isNode(node["name"]))
            io::throwParsingError(node, "No \"name\" entry in YAML");

        if (!io::isNode(node["configuration"]))
            io::throwParsingError(node, "No \"configuration\" entry in YAML");

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

        StatePtr state = std::make_shared<State>(robot);
        std::string name = node["name"].as<std::string>();

        for (const auto &joint : node["configuration"])
        {
            std::string frame = joint["frame"].as<std::string>();
            auto x = robot->getFrameValues(frame, state->data);
            auto v = io::toVector(joint["value"]);

            if (x.size() != v.size())
                io::throwParsingError(
                    joint, "Wrong number of values in frame \"%2%\" for named state \"%1%\"", name, frame);

            x = v;
        }

        robot->setNamedState(name, state);
    }

    /** \brief Parses a YAML node into a list of named states for a robot.
     *  \param[in,out] robot Robot to add states to.
     *  \param[in] node Node to parse.
     */
    void YAMLToStates(RobotPtr robot, const YAML::Node &node)
    {
        if (!io::isNode(node["states"]))
            return;

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

        for (const auto &state : node["states"])
            YAMLToNamedState(robot, state);
    }
}  // namespace

bool io::isNode(const YAML::Node &node)
{
    bool r = true;
    try
    {
        r = !node.IsNull();

        if (r)
            try
            {
                r = node.as<std::string>() != "~";
            }
            catch (std::exception &e)
            {
            }
    }
    catch (YAML::InvalidNode &e)
    {
        return false;
    }

    return r;
}

Eigen::VectorXd io::toVector(const YAML::Node &node)
{
    if (!node.IsSequence())
        io::throwParsingError(node, "Node is not a sequence");

    Eigen::VectorXd x(node.size());
    for (unsigned int i = 0; i < node.size(); ++i)
        x[i] = node[i].as<double>();

    return x;
}

YAML::Node io::toVector(const Eigen::Ref<const Eigen::VectorXd> &vec)
{
    YAML::Node node;
    for (unsigned int i = 0; i < vec.size(); ++i)
        node.push_back(vec[i]);

    return node;
}

RobotPtr io::loadRobot(const std::string &filename)
{
    const auto &result = io::loadFileToYAML(filename);
    if (!result.first)
    {
        SE2EZ_ERROR("Failed to load file \"%1%\"", filename);
        return nullptr;
    }

    try
    {
        auto robot = YAMLToRobot(result.second);
        YAMLToStates(robot, result.second);

        return robot;
    }
    catch (std::exception &e)
    {
        SE2EZ_ERROR("In YAML File %1%: %2%", io::resolvePath(filename), e.what());
        return nullptr;
    }
}

bool io::loadStates(RobotPtr robot, const std::string &filename)
{
    const auto &result = io::loadFileToYAML(filename);
    if (!result.first)
    {
        SE2EZ_ERROR("Failed to load file \"%1%\"", filename);
        return false;
    }

    try
    {
        YAMLToStates(robot, result.second);
        return true;
    }
    catch (std::exception &e)
    {
        SE2EZ_ERROR("In YAML File %1%: %2%", io::resolvePath(filename), e.what());
        return false;
    }
}