ROSEE::Parser Class Reference

Class responsible for parsing the YAML file providing the URDF, SRDF, the EE-HAL library implementation and the EEInterface. More...

#include <Parser.h>

Collaboration diagram for ROSEE::Parser:

Public Types
typedef std::shared_ptr< Parser >	Ptr
typedef std::shared_ptr< const Parser >	ConstPtr

Public Member Functions
	Parser (const ros::NodeHandle &nh)
virtual	~Parser ()
bool	init ()
	Initialization function using the ROS param ROSEEConfigPath. More...
bool	init (const std::string &urdf_path, const std::string &srdf_path, const std::string &action_path)
	Initialization function using the path_to_cfg parameter for the path. More...
std::map< std::string, urdf::JointConstSharedPtr >	getUrdfJointMap () const
	getter for the URDF information of the joints of the End-Effector More...
std::map< std::string, std::vector< std::string > >	getFingerJointMap () const
	getter for a description of the End-Effector as a map of finger name, finger joint names. More...
void	getFingerJointMap (std::map< std::string, std::vector< std::string >> &finger_joint_map) const
	getter for the map between the finger kinematic chains and the related actuated joints, reference version of getFingerJointMap More...
std::map< std::string, std::string >	getJointFingerMap () const
	getter for a description of the End-Effector as a map of joint name, finger name More...
void	getJointFingerMap (std::map< std::string, std::string > &joint_finger_map) const
	getter for a description of the End-Effector as a map of joint name, finger name More...
std::string	getEndEffectorName () const
	getter for the configure End-Effector name More...
int	getActuatedJointsNumber () const
	getter for the total number of actuated joints in the configuration files More...
void	printEndEffectorFingerJointsMap () const
	Utility to print the mapping between the End Effector finger chains and the related actuated joints. More...
std::string	getUrdfString () const
	get the whole urdf file parsed as a string More...
std::string	getSrdfString () const
	get the whole srdf file parsed as a string More...
std::string	getActionPath () const
	get the path where emit and parse grasping actions More...
std::string	getRoseeConfigPath () const
	get the filename (with path) of the yaml config file. More...

Private Member Functions
bool	configure ()
	configure the ROSEE parser based on the configration files requested More...
bool	parseURDF ()
	Function responsible to get the file needed to fill the internal data structure of the Parser with data coming from the ros_ee configuration file requested by the user. More...
bool	parseSRDF ()
	Function responsible to parse the SRDF data. More...
void	addNotInFingerJoints ()
bool	removePassiveJoints ()
	Function to remove the passive joints from the filled maps. More...
bool	getJointsInFinger (std::string base_link, std::string tip_link, std::string finger_name)
	fill a data structure related with the revolute/prismatic joints included in between base_link and tip_link in the requested URDF More...

Private Attributes
ros::NodeHandle	_nh
std::string	_urdf_path
std::string	_srdf_path
std::string	_urdf_string
std::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

Detailed Description

Class responsible for parsing the YAML file providing the URDF, SRDF, the EE-HAL library implementation and the EEInterface.

Definition at line 46 of file Parser.h.

Member Typedef Documentation

typedef std::shared_ptr<const Parser> ROSEE::Parser::ConstPtr

Definition at line 51 of file Parser.h.

typedef std::shared_ptr<Parser> ROSEE::Parser::Ptr

Definition at line 50 of file Parser.h.

Constructor & Destructor Documentation

ROSEE::Parser::Parser

(

const ros::NodeHandle &

nh

)

Definition at line 22 of file Parser.cpp.

                                              :
    _nh ( nh ) {

}

ROSEE::Parser::~Parser ( )

virtual

Definition at line 28 of file Parser.cpp.

                     {

}

Member Function Documentation

void ROSEE::Parser::addNotInFingerJoints ( )

private

Definition at line 172 of file Parser.cpp.

                                       {
    
    for (auto it : _urdf_model->joints_) { //this contains all joints
        
        if (it.second->mimic == nullptr) { //not a mimic joint...
        
            if (it.second->type == urdf::Joint::CONTINUOUS ||
                it.second->type == urdf::Joint::REVOLUTE ) {
                //it.second->type == urdf::Joint::PRISMATIC
            
                if (_urdf_joint_map.find(it.second->name) == _urdf_joint_map.end() ) {
                    
                    _urdf_joint_map.insert(std::make_pair(it.second->name, it.second));
                    _finger_joint_map["virtual_finger"].push_back ( it.second->name );
                    _joint_finger_map[it.second->name] = "virtual_finger";
                    _joints_num++;
                }
            }
        }
        
    }
    
}

bool ROSEE::Parser::configure ( )

private

configure the ROSEE parser based on the configration files requested

Returns

bool true on success, false otherwise

Definition at line 326 of file Parser.cpp.

                            {

    bool ret = true;
    //if ( getROSEndEffectorConfig() ) {

        if ( parseURDF() ) {
            
            if ( parseSRDF() ) {
                
                ROS_INFO_STREAM ( "ROSEndEffector Parser successfully configured using urdf file:  " << _urdf_path 
                    << "\n\t srdf file: " << _srdf_path << "\n\t actions folder " << _action_path
                );
            
            } else {
            
                ROS_ERROR_STREAM ( "ROSEndEffector Parser error while parsing SRDF");
                ret = false;
            }
            
        } else {
            
            ROS_ERROR_STREAM ( "ROSEndEffector Parser error while parsing URDF");
            ret = false;
        }

    //} else {

    //    ret = false;
    //}


    return ret;
}

std::string ROSEE::Parser::getActionPath

(

)

const

get the path where emit and parse grasping actions

Returns

the path as a string

Definition at line 464 of file Parser.cpp.

                                           {
    return _action_path;
};

int ROSEE::Parser::getActuatedJointsNumber

(

)

const

getter for the total number of actuated joints in the configuration files

Returns

int the number of actuated joints in the End-Effector

Definition at line 420 of file Parser.cpp.

                                               {
    
    return _joints_num;
}

std::string ROSEE::Parser::getEndEffectorName

(

)

const

getter for the configure End-Effector name

Returns

std::string the End-Effector name as reported in the URDF robot name

Definition at line 450 of file Parser.cpp.

                                                {

    return _urdf_model->getName();
}

std::map< std::string, std::vector< std::string > > ROSEE::Parser::getFingerJointMap

(

)

const

getter for a description of the End-Effector as a map of finger name, finger joint names.

There exists another version to get the map as reference: getFingerJointMapAsReference

Returns

std::map<std::string, std::vector<std::string>> a map respresenting an End-Effector with a key representing the finger name and the values representing a vector of joint names.

Definition at line 425 of file Parser.cpp.

                                                                                {

    return _finger_joint_map;
}

void ROSEE::Parser::getFingerJointMap

(

std::map< std::string, std::vector< std::string >> &

finger_joint_map

)

const

getter for the map between the finger kinematic chains and the related actuated joints, reference version of getFingerJointMap

Parameters

finger_joint_map

a map between the finger kinematic chains and the related actuated joints

Returns

void

Definition at line 440 of file Parser.cpp.

                                                                                                        {

    finger_joint_map = _finger_joint_map;
}

std::map< std::string, std::string > ROSEE::Parser::getJointFingerMap

(

)

const

getter for a description of the End-Effector as a map of joint name, finger name

Returns

std::map<std::string, std::string> a map respresenting an End-Effector with a key representing the joint name and the value representing the finger which the joint belongs to

Definition at line 430 of file Parser.cpp.

                                                                    {
    
    return _joint_finger_map;
}

void ROSEE::Parser::getJointFingerMap

(

std::map< std::string, std::string > &

joint_finger_map

)

const

getter for a description of the End-Effector as a map of joint name, finger name

Parameters

joint_finger_map

a map between the joint and the related finger kinematic chain

Returns

void

Definition at line 445 of file Parser.cpp.

                                                                                             {

    joint_finger_map = _joint_finger_map;
}

bool ROSEE::Parser::getJointsInFinger ( std::string  base_link, std::string  tip_link, std::string  finger_name  )

private

fill a data structure related with the revolute/prismatic joints included in between base_link and tip_link in the requested URDF

Parameters

base_link	base link of the finger chain
tip_link	tip link of the finger chain

Returns

bool true if the chain is found in the URDF, false otherwise

Definition at line 32 of file Parser.cpp.

                                        {

    //we add here the finger to the map because later is added only if the joint is actuated.
    //Instead we want in the map all the fingers, even if they have no actuated joint in.
    //this is necessary to not cause later problem due to have the right number of finger
    _finger_joint_map.insert(std::make_pair(finger_name, std::vector<std::string>())); 
    
    
    KDL::Chain actual_chain;
    if ( _robot_tree.getChain ( base_link, tip_link, actual_chain ) ) {

        int segments_num = actual_chain.getNrOfSegments();
        for ( int i = 0; i < segments_num; i++ ) {

            KDL::Segment actual_segment = actual_chain.getSegment ( i );
            KDL::Joint actual_joint = actual_segment.getJoint();

            bool is_valid_joint = false;

            // Check if joint is revolute or prismatic
            is_valid_joint = actual_joint.getTypeName() == "RotAxis";   
            
//                              || actual_joint.getTypeName() == "TransAxis";
            
            auto urdf_joint = _urdf_model->getJoint(actual_joint.getName());
            bool is_mimic_joint = (urdf_joint->mimic == nullptr) ? false : true;
            

            // if the joint is revolute or prismatic AND not a mimic (mimic joint is a not actuated joint)
            if ( is_valid_joint && (!is_mimic_joint) ) {

                _finger_joint_map[finger_name].push_back ( actual_joint.getName() );
                _joint_finger_map[actual_joint.getName()] = finger_name;
                _urdf_joint_map[actual_joint.getName()] = _urdf_model->getJoint ( actual_joint.getName() );

                _joints_num++;

            }
        }

        return true;
    }

    ROS_ERROR_STREAM ( "chain from base_link " << base_link << " to tip_link " << tip_link << " not found in the URDF" );

    return false;
}

std::string ROSEE::Parser::getRoseeConfigPath

(

)

const

get the filename (with path) of the yaml config file.

Useful get to print infos about file parsed outside this class

Returns

a string that is the filepath of yaml config file

std::string ROSEE::Parser::getSrdfString

(

)

const

get the whole srdf file parsed as a string

Returns

a string containg the srdf file parsed

Definition at line 459 of file Parser.cpp.

                                           {
    return _srdf_string;
}

std::map< std::string, urdf::JointConstSharedPtr > ROSEE::Parser::getUrdfJointMap

(

)

const

getter for the URDF information of the joints of the End-Effector

Returns

std::map<std::string, urdf::JointConstSharedPtr> map between the joint name and the URDF info of the joint

Definition at line 435 of file Parser.cpp.

                                                                                {

    return _urdf_joint_map;
}

std::string ROSEE::Parser::getUrdfString

(

)

const

get the whole urdf file parsed as a string

Returns

a string containg the urdf file parsed

Definition at line 455 of file Parser.cpp.

                                           {
    return _urdf_string;
}

bool ROSEE::Parser::init

(

)

Initialization function using the ROS param ROSEEConfigPath.

Returns

bool true for success, false otherwise

Definition at line 362 of file Parser.cpp.

                       {

    // try to retrive the path to config from the ROS param server TBD namespace should be take into account
    if ( _nh.getParam ( "/urdf_path", _urdf_path ) && 
         _nh.getParam ( "/srdf_path", _srdf_path ) &&
         _nh.getParam ( "/actions_folder_path", _action_path )
    ) {

        _is_initialized =  configure();
        return _is_initialized;
    }

    // error
    ROS_ERROR_STREAM ( "in " << __func__ << " : '_urdf_path' and/or '_srdf_path' and/or 'actions_folder_path' not found on ROS parameter server" );
    return false;

}

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

Initialization function using the path_to_cfg parameter for the path.

Parameters

urdf_path	path to the urdf file
srdf_path	path to the srdf file
action_path	path to the action folder

Returns

bool true for success, false otherwise

Definition at line 380 of file Parser.cpp.

                                                                                                              {

    _urdf_path = urdf_path;
    _srdf_path = srdf_path;
    _action_path = action_path;

    _is_initialized =  configure();
    return _is_initialized;
}

bool ROSEE::Parser::parseSRDF ( )

private

Function responsible to parse the SRDF data.

Returns

bool true if the SRDF requested exists, false otherwise

Definition at line 85 of file Parser.cpp.

                            {

    // initialize srdfdom
    _srdfdom.initFile ( *_urdf_model, _srdf_path );

    // get the end-effectors in the SRDF file
    std::vector<srdf::Model::EndEffector> srdf_end_effectors = _srdfdom.getEndEffectors();

    // NOTE only one end-effectors supported
    // TBD support multiple end-effectors
    if ( srdf_end_effectors.size() != 1 ) {

        return false;
    }

    ROS_INFO_STREAM ( "ros_end_effector Parser found end_effector: " << srdf_end_effectors.at ( 0 ).name_ );

    // get all the groups in the SRDF
    std::vector<srdf::Model::Group> srdf_groups = _srdfdom.getGroups();

    // TBD will be a vector
    srdf::Model::Group fingers_group;
    // find end effector fingers chains group
    // TBD will be a vector
    int group_num = srdf_groups.size();
    // TBD will be a vector
    std::string end_effector_group_name = srdf_end_effectors.at ( 0 ).component_group_;
    for ( int i = 0; i < group_num; i++ ) {
        if ( srdf_groups[i].name_ == end_effector_group_name ) {
            fingers_group = srdf_groups[i];
            ROS_INFO_STREAM ( "ros_end_effector Parser found group: " << end_effector_group_name << " in the SRDF with the following fingers: " );
        }
    }

    _fingers_num = fingers_group.subgroups_.size();
    _fingers_names.resize ( _fingers_num );
    _fingers_group_id.resize ( _fingers_num );

    // fill the chain names vector
    for ( int i = 0; i < _fingers_num; i++ ) {

        _fingers_names[i] = fingers_group.subgroups_[i];

        // find the id of the current finger group
        for ( int j = 0; j < group_num; j++ ) {
            if ( srdf_groups[j].name_ == _fingers_names[i] ) {
                _fingers_group_id[i] = j;
            }
        }
        ROS_INFO_STREAM ( _fingers_names[i] );
    }

    // iterate over the fingers and find revolute joints
    for ( int i = 0; i < _fingers_num; i++ ) {
        srdf::Model::Group current_finger_group = srdf_groups[ _fingers_group_id[i] ];

        // NOTE only one chain per group
        if ( current_finger_group.chains_.size() != CHAIN_PER_GROUP )  {

            ROS_ERROR_STREAM ( "for the finger chain groups you can specify only one chain per group in your SRDF check " << 
                               current_finger_group.name_.c_str() << " group" );
            return false;
        }

        // fill the enabled/disabled joints in chain map
        if ( !getJointsInFinger ( current_finger_group.chains_[0].first,
                                  current_finger_group.chains_[0].second,
                                  current_finger_group.name_
                                ) ) {
            return false;
        }
    }
    
    addNotInFingerJoints();
    
    removePassiveJoints();

    // save srdf as string 
    std::ifstream t_srdf ( _srdf_path );
    std::stringstream buffer_srdf;
    buffer_srdf << t_srdf.rdbuf();
    _srdf_string = buffer_srdf.str();
    
    return true;

}

bool ROSEE::Parser::parseURDF ( )

private

Function responsible to get the file needed to fill the internal data structure of the Parser with data coming from the ros_ee configuration file requested by the user.

Returns

bool true on success, false if the config file contains errors (either in the main YAML, or in URDF, or in SRDF or in EE-HAL library path. or in the Grasping Primitive Definition). Function responsible to parse the URDF data

bool true if the URDF requested exists, false otherwise

Definition at line 225 of file Parser.cpp.

                            {

    std::string xml_string;
    std::fstream xml_file ( _urdf_path.c_str(), std::fstream::in );
    if ( xml_file.is_open() ) {

        while ( xml_file.good() ) {

            std::string line;
            std::getline ( xml_file, line );
            xml_string += ( line + "\n" );
        }

        xml_file.close();
        _urdf_model = urdf::parseURDF ( xml_string );

        // create the robot KDL tree from the URDF model
        if ( !kdl_parser::treeFromUrdfModel ( *_urdf_model, _robot_tree ) ) {

            ROS_ERROR_STREAM ( "in " << __func__ << " Failed to construct kdl tree" );
            return false;
        }

        // save urdf and string (can be useful to have, thanks @alaurenzi!)
        std::ifstream t_urdf ( _urdf_path );
        std::stringstream buffer_urdf;
        buffer_urdf << t_urdf.rdbuf();
        _urdf_string = buffer_urdf.str();

        return true;

    } else {

        ROS_ERROR_STREAM ( "in " << __func__ << " : Can NOT open " << _urdf_path << " !" );
        return false;
    }
}

void ROSEE::Parser::printEndEffectorFingerJointsMap

(

)

const

Utility to print the mapping between the End Effector finger chains and the related actuated joints.

Returns

void

Definition at line 390 of file Parser.cpp.

                                                        {

    if ( _is_initialized ) {

        ROS_INFO_STREAM ( "ROS End Effector: Finger Joints Map" );
        ROS_INFO_STREAM ( "-------------------------" );
        for ( auto& chain_joints: _finger_joint_map ) {
            ROS_INFO_STREAM ( chain_joints.first );

            int nJointInFinger = chain_joints.second.size();
            
            if ( nJointInFinger == 0 ) {
                
                ROS_INFO_STREAM ( "No actuated joint in this finger" );
                
            } else {
                
                for ( int i = 0; i < nJointInFinger ; i++ ) {
                    ROS_INFO_STREAM ( chain_joints.second.at ( i ) );
                }
            }

            ROS_INFO_STREAM ( "-------------------------" );
        }
    } else {

        ROS_ERROR_STREAM ( "in " << __func__ << " :  ROSEE::Parser needs to be initialized. Call init() frist." );
    }
}

bool ROSEE::Parser::removePassiveJoints ( )

private

Function to remove the passive joints from the filled maps.

Returns

bool is everything works correctly

Definition at line 196 of file Parser.cpp.

                                      {
    
    std::vector<srdf::Model::PassiveJoint> passiveJointList = _srdfdom.getPassiveJoints();
    
    for (auto passiveJoint : passiveJointList) {
        //the passive can be also already deleted so we chech if we delete it(e.g. it is also mimic)
        if (_urdf_joint_map.erase(passiveJoint.name_) > 0) {
            
            //we have also to remove it from the other maps
            std::string fingerOfPassive = _joint_finger_map.at(passiveJoint.name_);
            _joint_finger_map.erase(passiveJoint.name_);
            
            for (auto it = _finger_joint_map.at(fingerOfPassive).begin(); 
                 it!= _finger_joint_map.at(fingerOfPassive).end(); ++it){
                
                if (it->compare(passiveJoint.name_) == 0 ) {
                    _finger_joint_map.at(fingerOfPassive).erase(it);
                    break;
                }
            }

            _joints_num--;
        }   
    }

    return true;
}

Member Data Documentation

std::string ROSEE::Parser::_action_path

private

Definition at line 173 of file Parser.h.

std::map<std::string, std::vector<std::string> > ROSEE::Parser::_finger_joint_map

private

Definition at line 187 of file Parser.h.

std::vector<int> ROSEE::Parser::_fingers_group_id

private

Definition at line 183 of file Parser.h.

std::vector<std::string> ROSEE::Parser::_fingers_names

private

Definition at line 182 of file Parser.h.

int ROSEE::Parser::_fingers_num = 0

private

Definition at line 181 of file Parser.h.

bool ROSEE::Parser::_is_initialized = false

private

Definition at line 174 of file Parser.h.

std::map<std::string, std::string> ROSEE::Parser::_joint_finger_map

private

Definition at line 188 of file Parser.h.

int ROSEE::Parser::_joints_num = 0

private

Definition at line 185 of file Parser.h.

ros::NodeHandle ROSEE::Parser::_nh

private

Definition at line 171 of file Parser.h.

KDL::Tree ROSEE::Parser::_robot_tree

private

Definition at line 177 of file Parser.h.

std::string ROSEE::Parser::_srdf_path

private

Definition at line 172 of file Parser.h.

std::string ROSEE::Parser::_srdf_string

private

Definition at line 172 of file Parser.h.

srdf::Model ROSEE::Parser::_srdfdom

private

Definition at line 179 of file Parser.h.

std::map<std::string, urdf::JointConstSharedPtr> ROSEE::Parser::_urdf_joint_map

private

Definition at line 189 of file Parser.h.

urdf::ModelInterfaceSharedPtr ROSEE::Parser::_urdf_model

private

Definition at line 176 of file Parser.h.

std::string ROSEE::Parser::_urdf_path

private

Definition at line 172 of file Parser.h.

std::string ROSEE::Parser::_urdf_string

private

Definition at line 172 of file Parser.h.

---

The documentation for this class was generated from the following files:

• /home/travis/build/ADVRHumanoids/ROSEndEffector/include/end_effector/Parser.h
• /home/travis/build/ADVRHumanoids/ROSEndEffector/src/Parser.cpp