Parser.h

Go to the documentation of this file.

/*
 * Copyright (C) 2019 IIT-HHCM
 * Author: Luca Muratore
 * email:  luca.muratore@iit.it
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
*/

#ifndef __ROSEE_PARSER__
#define __ROSEE_PARSER__


#include <iostream>
#include <fstream>
#include <memory>

#include <ros/ros.h>
#include <ros/console.h>

#include <end_effector/Utils.h>

#include <yaml-cpp/yaml.h>
#include <urdf_parser/urdf_parser.h>
#include <kdl_parser/kdl_parser.hpp>
#include <srdfdom/model.h>

//to find relative path for the config files
#include <boost/filesystem/path.hpp>

namespace ROSEE {
    
    class Parser {
            
    public:
        
        typedef std::shared_ptr<Parser> Ptr;
        typedef std::shared_ptr<const Parser> ConstPtr;

        Parser( const ros::NodeHandle& nh );
        //Parser ( const Parser& other );
        //Parser& operator= ( const Parser& p );
        virtual ~Parser();

        bool init ();
        
        bool init (const std::string& urdf_path, const std::string& srdf_path, const std::string& action_path );

        
        std::map<std::string, urdf::JointConstSharedPtr> getUrdfJointMap() const;
        
        std::map<std::string, std::vector<std::string>> getFingerJointMap() const;
        
        void getFingerJointMap(std::map<std::string, std::vector<std::string>>& finger_joint_map) const;
        
        std::map<std::string, std::string> getJointFingerMap() const;
        
        void getJointFingerMap(std::map<std::string, std::string>& joint_finger_map) const;
        
        std::string getEndEffectorName() const;
        
        int getActuatedJointsNumber() const;
        
        void printEndEffectorFingerJointsMap() const;
        
        std::string getUrdfString() const;
        
        std::string getSrdfString() const;
        
        std::string getActionPath() const;
        
        std::string getRoseeConfigPath() const;
        
    private:
        
        ros::NodeHandle _nh;
        std::string _urdf_path, _srdf_path, _urdf_string, _srdf_string;
        std::string _action_path;
        bool _is_initialized = false;
        
        urdf::ModelInterfaceSharedPtr _urdf_model;
        KDL::Tree _robot_tree;
        
        srdf::Model _srdfdom;
        
        int _fingers_num = 0;
        std::vector<std::string> _fingers_names;
        std::vector<int> _fingers_group_id;
        
        int _joints_num = 0;
        
        std::map<std::string, std::vector<std::string>> _finger_joint_map;
        std::map<std::string, std::string> _joint_finger_map;
        std::map<std::string, urdf::JointConstSharedPtr> _urdf_joint_map;
        
        
        bool configure();
        
        //bool getROSEndEffectorConfig();
        
        bool parseURDF();
        
        bool parseSRDF();
        
        void addNotInFingerJoints();
        
        bool removePassiveJoints();
        
        bool getJointsInFinger(std::string base_link, std::string tip_link, std::string finger_name);

        
    };
}

#endif // __ROSEE_PARSER__