UrdfTraverser.h

Go to the documentation of this file.

#ifndef URDF_TRAVERSER_URDFTRAVERSER_H
#define URDF_TRAVERSER_URDFTRAVERSER_H
// Copyright Jennifer Buehler

//-----------------------------------------------------


#include <urdf_traverser/Types.h>
#include <urdf_traverser/RecursionParams.h>

#include <iostream>
#include <string>
#include <map>
#include <vector>

#define BOOST_NO_CXX11_SCOPED_ENUMS
#include <boost/filesystem.hpp>
#undef BOOST_NO_CXX11_SCOPED_ENUMS


namespace urdf_traverser
{

class UrdfTraverser
{
public:

    explicit UrdfTraverser():
        model(new urdf::Model())
    {}

    ~UrdfTraverser()
    {
    }

    bool getModelFromFile(const std::string& urdfFilename, std::string& xml_string) const;

    bool loadModelFromFile(const std::string& urdfFilename);

    bool loadModelFromXMLString(const std::string& xmlString);

    inline void setModelDirectory(const std::string& dir)
    {
        // make sure this is an absolute canonical path
        boost::filesystem::path _dir(dir);
        boost::filesystem::path _absDir = boost::filesystem::canonical(_dir);
        modelDir = _absDir.string();
    }
    inline std::string getModelDirectory() const
    {
        return modelDir;
    }

    //bool loadModelFromParameterServer();

    void printJointNames(const std::string& fromLink);

    std::string getRootLinkName() const;

    std::string getModelName() const
    {
        return this->model->getName();
    }

    bool printModel(const std::string& fromLink, bool verbose);

    bool printModel(bool verbose);


    int traverseTreeTopDown(const std::string& linkName, boost::function< int(RecursionParamsPtr&)> link_cb,
                            RecursionParamsPtr& params, bool includeLink = true);
    int traverseTreeBottomUp(const std::string& linkName,
                             boost::function< int(RecursionParamsPtr&)> link_cb,
                             RecursionParamsPtr& params, bool includeLink = true);


    JointPtr getJoint(const std::string& name);
    JointConstPtr readJoint(const std::string& name) const;
    LinkPtr getLink(const std::string& name);
    LinkConstPtr readLink(const std::string& name) const;

    LinkPtr getChildLink(const JointConstPtr& joint);
    LinkConstPtr readChildLink(const JointConstPtr& joint) const;

    JointPtr getParentJoint(const JointConstPtr& joint);
    JointConstPtr readParentJoint(const JointConstPtr& joint) const;


    ModelPtr getModel()
    {
        return model;
    }
    ModelConstPtr readModel() const
    {
        return model;
    }

    bool getJointNames(const std::string& fromLink,
                       const bool skipFixed, std::vector<std::string>& result);


protected:

    bool getDependencyOrderedJoints(std::vector<JointPtr>& result, const JointPtr& from_joint,
                                    bool allowSplits = true, bool onlyActive = true);

    bool getDependencyOrderedJoints(std::vector<JointPtr>& result, const LinkPtr& from_link,
                                    bool allowSplits = true, bool onlyActive = true);

    int getChildJoint(const JointPtr& joint, JointPtr& child);

    int traverseTreeTopDown(const LinkPtr& link, boost::function< int(RecursionParamsPtr&)> link_cb,
                            RecursionParamsPtr& params, bool includeLink = true, unsigned int level = 0);

    int traverseTreeBottomUp(const LinkPtr& link, boost::function<int(RecursionParamsPtr&)> link_cb,
                             RecursionParamsPtr& params, bool includeLink = true, unsigned int level = 0);

    bool hasChildLink(const LinkConstPtr& link, const std::string& childName) const;


    EigenTransform getTransform(const LinkPtr& from_link,  const JointPtr& to_joint);

private:

    int printLink(RecursionParamsPtr& p);

    ModelPtr model;

    std::string modelDir;
};

}  //  namespace urdf_traverser
#endif   // URDF_TRAVERSER_URDFTRAVERSER_H