ROSEndEffector
ROS End-Effector package: provides a ROS-based set of standard interfaces to command robotics end-effectors in an agnostic fashion.
Parser.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 IIT-HHCM
3  * Author: Luca Muratore
4  * email: luca.muratore@iit.it
5  *
6  * Licensed under the Apache License, Version 2.0 (the "License");
7  * you may not use this file except in compliance with the License.
8  * You may obtain a copy of the License at
9  * http://www.apache.org/licenses/LICENSE-2.0
10  *
11  * Unless required by applicable law or agreed to in writing, software
12  * distributed under the License is distributed on an "AS IS" BASIS,
13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14  * See the License for the specific language governing permissions and
15  * limitations under the License.
16 */
17 
18 #include <end_effector/Parser.h>
19 
20 #define CHAIN_PER_GROUP 1
21 
22 ROSEE::Parser::Parser ( const ros::NodeHandle& nh ) :
23  _nh ( nh ) {
24 
25 }
26 
27 
29 
30 }
31 
32 bool ROSEE::Parser::getJointsInFinger ( std::string base_link,
33  std::string tip_link,
34  std::string finger_name
35  ) {
36 
37  //we add here the finger to the map because later is added only if the joint is actuated.
38  //Instead we want in the map all the fingers, even if they have no actuated joint in.
39  //this is necessary to not cause later problem due to have the right number of finger
40  _finger_joint_map.insert(std::make_pair(finger_name, std::vector<std::string>()));
41 
42 
43  KDL::Chain actual_chain;
44  if ( _robot_tree.getChain ( base_link, tip_link, actual_chain ) ) {
45 
46  int segments_num = actual_chain.getNrOfSegments();
47  for ( int i = 0; i < segments_num; i++ ) {
48 
49  KDL::Segment actual_segment = actual_chain.getSegment ( i );
50  KDL::Joint actual_joint = actual_segment.getJoint();
51 
52  bool is_valid_joint = false;
53 
54  // Check if joint is revolute or prismatic
55  is_valid_joint = actual_joint.getTypeName() == "RotAxis";
56 
57 // || actual_joint.getTypeName() == "TransAxis";
58 
59  auto urdf_joint = _urdf_model->getJoint(actual_joint.getName());
60  bool is_mimic_joint = (urdf_joint->mimic == nullptr) ? false : true;
61 
62 
63  // if the joint is revolute or prismatic AND not a mimic (mimic joint is a not actuated joint)
64  if ( is_valid_joint && (!is_mimic_joint) ) {
65 
66  _finger_joint_map[finger_name].push_back ( actual_joint.getName() );
67  _joint_finger_map[actual_joint.getName()] = finger_name;
68  _urdf_joint_map[actual_joint.getName()] = _urdf_model->getJoint ( actual_joint.getName() );
69 
70  _joints_num++;
71 
72  }
73  }
74 
75  return true;
76  }
77 
78  ROS_ERROR_STREAM ( "chain from base_link " << base_link << " to tip_link " << tip_link << " not found in the URDF" );
79 
80  return false;
81 }
82 
83 
84 
86 
87  // initialize srdfdom
88  _srdfdom.initFile ( *_urdf_model, _srdf_path );
89 
90  // get the end-effectors in the SRDF file
91  std::vector<srdf::Model::EndEffector> srdf_end_effectors = _srdfdom.getEndEffectors();
92 
93  // NOTE only one end-effectors supported
94  // TBD support multiple end-effectors
95  if ( srdf_end_effectors.size() != 1 ) {
96 
97  return false;
98  }
99 
100  ROS_INFO_STREAM ( "ros_end_effector Parser found end_effector: " << srdf_end_effectors.at ( 0 ).name_ );
101 
102  // get all the groups in the SRDF
103  std::vector<srdf::Model::Group> srdf_groups = _srdfdom.getGroups();
104 
105  // TBD will be a vector
106  srdf::Model::Group fingers_group;
107  // find end effector fingers chains group
108  // TBD will be a vector
109  int group_num = srdf_groups.size();
110  // TBD will be a vector
111  std::string end_effector_group_name = srdf_end_effectors.at ( 0 ).component_group_;
112  for ( int i = 0; i < group_num; i++ ) {
113  if ( srdf_groups[i].name_ == end_effector_group_name ) {
114  fingers_group = srdf_groups[i];
115  ROS_INFO_STREAM ( "ros_end_effector Parser found group: " << end_effector_group_name << " in the SRDF with the following fingers: " );
116  }
117  }
118 
119  _fingers_num = fingers_group.subgroups_.size();
120  _fingers_names.resize ( _fingers_num );
121  _fingers_group_id.resize ( _fingers_num );
122 
123  // fill the chain names vector
124  for ( int i = 0; i < _fingers_num; i++ ) {
125 
126  _fingers_names[i] = fingers_group.subgroups_[i];
127 
128  // find the id of the current finger group
129  for ( int j = 0; j < group_num; j++ ) {
130  if ( srdf_groups[j].name_ == _fingers_names[i] ) {
131  _fingers_group_id[i] = j;
132  }
133  }
134  ROS_INFO_STREAM ( _fingers_names[i] );
135  }
136 
137  // iterate over the fingers and find revolute joints
138  for ( int i = 0; i < _fingers_num; i++ ) {
139  srdf::Model::Group current_finger_group = srdf_groups[ _fingers_group_id[i] ];
140 
141  // NOTE only one chain per group
142  if ( current_finger_group.chains_.size() != CHAIN_PER_GROUP ) {
143 
144  ROS_ERROR_STREAM ( "for the finger chain groups you can specify only one chain per group in your SRDF check " <<
145  current_finger_group.name_.c_str() << " group" );
146  return false;
147  }
148 
149  // fill the enabled/disabled joints in chain map
150  if ( !getJointsInFinger ( current_finger_group.chains_[0].first,
151  current_finger_group.chains_[0].second,
152  current_finger_group.name_
153  ) ) {
154  return false;
155  }
156  }
157 
159 
161 
162  // save srdf as string
163  std::ifstream t_srdf ( _srdf_path );
164  std::stringstream buffer_srdf;
165  buffer_srdf << t_srdf.rdbuf();
166  _srdf_string = buffer_srdf.str();
167 
168  return true;
169 
170 }
171 
173 
174  for (auto it : _urdf_model->joints_) { //this contains all joints
175 
176  if (it.second->mimic == nullptr) { //not a mimic joint...
177 
178  if (it.second->type == urdf::Joint::CONTINUOUS ||
179  it.second->type == urdf::Joint::REVOLUTE ) {
180  //it.second->type == urdf::Joint::PRISMATIC
181 
182  if (_urdf_joint_map.find(it.second->name) == _urdf_joint_map.end() ) {
183 
184  _urdf_joint_map.insert(std::make_pair(it.second->name, it.second));
185  _finger_joint_map["virtual_finger"].push_back ( it.second->name );
186  _joint_finger_map[it.second->name] = "virtual_finger";
187  _joints_num++;
188  }
189  }
190  }
191 
192  }
193 
194 }
195 
197 
198  std::vector<srdf::Model::PassiveJoint> passiveJointList = _srdfdom.getPassiveJoints();
199 
200  for (auto passiveJoint : passiveJointList) {
201  //the passive can be also already deleted so we chech if we delete it(e.g. it is also mimic)
202  if (_urdf_joint_map.erase(passiveJoint.name_) > 0) {
203 
204  //we have also to remove it from the other maps
205  std::string fingerOfPassive = _joint_finger_map.at(passiveJoint.name_);
206  _joint_finger_map.erase(passiveJoint.name_);
207 
208  for (auto it = _finger_joint_map.at(fingerOfPassive).begin();
209  it!= _finger_joint_map.at(fingerOfPassive).end(); ++it){
210 
211  if (it->compare(passiveJoint.name_) == 0 ) {
212  _finger_joint_map.at(fingerOfPassive).erase(it);
213  break;
214  }
215  }
216 
217  _joints_num--;
218  }
219  }
220 
221  return true;
222 }
223 
224 
226 
227  std::string xml_string;
228  std::fstream xml_file ( _urdf_path.c_str(), std::fstream::in );
229  if ( xml_file.is_open() ) {
230 
231  while ( xml_file.good() ) {
232 
233  std::string line;
234  std::getline ( xml_file, line );
235  xml_string += ( line + "\n" );
236  }
237 
238  xml_file.close();
239  _urdf_model = urdf::parseURDF ( xml_string );
240 
241  // create the robot KDL tree from the URDF model
242  if ( !kdl_parser::treeFromUrdfModel ( *_urdf_model, _robot_tree ) ) {
243 
244  ROS_ERROR_STREAM ( "in " << __func__ << " Failed to construct kdl tree" );
245  return false;
246  }
247 
248  // save urdf and string (can be useful to have, thanks @alaurenzi!)
249  std::ifstream t_urdf ( _urdf_path );
250  std::stringstream buffer_urdf;
251  buffer_urdf << t_urdf.rdbuf();
252  _urdf_string = buffer_urdf.str();
253 
254  return true;
255 
256  } else {
257 
258  ROS_ERROR_STREAM ( "in " << __func__ << " : Can NOT open " << _urdf_path << " !" );
259  return false;
260  }
261 }
262 
263 /*
264 bool ROSEE::Parser::getROSEndEffectorConfig() {
265 
266  bool success = true;
267 
268  // check if the ROSEE config path exists
269  std::ifstream fin ( _ros_ee_config_path );
270  if ( fin.fail() ) {
271 
272  ROS_ERROR_STREAM ( "in " << __func__ << " : Can NOT open " << _ros_ee_config_path << "!" );
273  success = false;
274  } else {
275 
276  // load the node for the _ros_ee_config_path
277  YAML::Node cfg = YAML::LoadFile ( _ros_ee_config_path );
278 
279  // find the internal node ros_end_effector
280  YAML::Node ros_ee_node;
281  if ( cfg["ros_end_effector"] ) {
282 
283  ros_ee_node = cfg["ros_end_effector"];
284  } else {
285 
286  ROS_ERROR_STREAM ( "in " << __func__ << " : YAML file " << _ros_ee_config_path << " does not contain ros_end_effector mandatory node!!" );
287  success = false;
288  }
289 
290  // check the urdf_filename
291  if ( ros_ee_node["urdf_path"] ) {
292 
293  // TBD relative path in more elegant way
294  _urdf_path = ROSEE::Utils::getPackagePath() + "/configs/" + ros_ee_node["urdf_path"].as<std::string>();
295  ROS_INFO_STREAM ( "ros_end_effector Parser found URDF path: " << _urdf_path );
296  } else {
297 
298  ROS_ERROR_STREAM ( "in " << __func__ << " : ros_end_effector node of " << _ros_ee_config_path << " does not contain urdf_path mandatory node!!" );
299  success = false;
300  }
301 
302  // check the srdf_filename
303  if ( ros_ee_node["srdf_path"] ) {
304 
305  // TBD relative path in more elegant way
306  _srdf_path = ROSEE::Utils::getPackagePath() + "/configs/" + ros_ee_node["srdf_path"].as<std::string>();
307  ROS_INFO_STREAM ( "ros_end_effector Parser found SRDF path: " << _srdf_path );
308  } else {
309 
310  ROS_ERROR_STREAM ( "in " << __func__ << " : ros_end_effector node of " << _ros_ee_config_path << " does not contain srdf_path mandatory node!!" );
311  success = false;
312  }
313 
314  if ( ros_ee_node["action_path"] ) {
315  _action_path = ROSEE::Utils::getPackagePath() + "/configs/" + ros_ee_node["action_path"].as<std::string>();
316  ROS_INFO_STREAM ( "ros_end_effector Parser found config folder for actions: " << _action_path );
317 
318  }
319  }
320 
321  return success;
322 }
323 */
324 
325 
327 
328  bool ret = true;
329  //if ( getROSEndEffectorConfig() ) {
330 
331  if ( parseURDF() ) {
332 
333  if ( parseSRDF() ) {
334 
335  ROS_INFO_STREAM ( "ROSEndEffector Parser successfully configured using urdf file: " << _urdf_path
336  << "\n\t srdf file: " << _srdf_path << "\n\t actions folder " << _action_path
337  );
338 
339  } else {
340 
341  ROS_ERROR_STREAM ( "ROSEndEffector Parser error while parsing SRDF");
342  ret = false;
343  }
344 
345  } else {
346 
347  ROS_ERROR_STREAM ( "ROSEndEffector Parser error while parsing URDF");
348  ret = false;
349  }
350 
351  //} else {
352 
353  // ret = false;
354  //}
355 
356 
357  return ret;
358 }
359 
360 
361 
363 
364  // try to retrive the path to config from the ROS param server TBD namespace should be take into account
365  if ( _nh.getParam ( "/urdf_path", _urdf_path ) &&
366  _nh.getParam ( "/srdf_path", _srdf_path ) &&
367  _nh.getParam ( "/actions_folder_path", _action_path )
368  ) {
369 
371  return _is_initialized;
372  }
373 
374  // error
375  ROS_ERROR_STREAM ( "in " << __func__ << " : '_urdf_path' and/or '_srdf_path' and/or 'actions_folder_path' not found on ROS parameter server" );
376  return false;
377 
378 }
379 
380 bool ROSEE::Parser::init ( const std::string& urdf_path, const std::string& srdf_path, const std::string& action_path ) {
381 
382  _urdf_path = urdf_path;
383  _srdf_path = srdf_path;
384  _action_path = action_path;
385 
387  return _is_initialized;
388 }
389 
391 
392  if ( _is_initialized ) {
393 
394  ROS_INFO_STREAM ( "ROS End Effector: Finger Joints Map" );
395  ROS_INFO_STREAM ( "-------------------------" );
396  for ( auto& chain_joints: _finger_joint_map ) {
397  ROS_INFO_STREAM ( chain_joints.first );
398 
399  int nJointInFinger = chain_joints.second.size();
400 
401  if ( nJointInFinger == 0 ) {
402 
403  ROS_INFO_STREAM ( "No actuated joint in this finger" );
404 
405  } else {
406 
407  for ( int i = 0; i < nJointInFinger ; i++ ) {
408  ROS_INFO_STREAM ( chain_joints.second.at ( i ) );
409  }
410  }
411 
412  ROS_INFO_STREAM ( "-------------------------" );
413  }
414  } else {
415 
416  ROS_ERROR_STREAM ( "in " << __func__ << " : ROSEE::Parser needs to be initialized. Call init() frist." );
417  }
418 }
419 
421 
422  return _joints_num;
423 }
424 
425 std::map< std::string, std::vector< std::string >> ROSEE::Parser::getFingerJointMap() const {
426 
427  return _finger_joint_map;
428 }
429 
430 std::map< std::string, std::string > ROSEE::Parser::getJointFingerMap() const {
431 
432  return _joint_finger_map;
433 }
434 
435 std::map< std::string, urdf::JointConstSharedPtr > ROSEE::Parser::getUrdfJointMap() const {
436 
437  return _urdf_joint_map;
438 }
439 
440 void ROSEE::Parser::getFingerJointMap( std::map< std::string, std::vector< std::string >>& finger_joint_map ) const {
441 
442  finger_joint_map = _finger_joint_map;
443 }
444 
445 void ROSEE::Parser::getJointFingerMap ( std::map< std::string, std::string >& joint_finger_map ) const {
446 
447  joint_finger_map = _joint_finger_map;
448 }
449 
451 
452  return _urdf_model->getName();
453 }
454 
455 std::string ROSEE::Parser::getUrdfString() const {
456  return _urdf_string;
457 }
458 
459 std::string ROSEE::Parser::getSrdfString() const {
460  return _srdf_string;
461 }
462 
463 
464 std::string ROSEE::Parser::getActionPath() const {
465  return _action_path;
466 };
467 
468 
469 
470 
#define CHAIN_PER_GROUP
Definition: Parser.cpp:20
std::string getSrdfString() const
get the whole srdf file parsed as a string
Definition: Parser.cpp:459
void printEndEffectorFingerJointsMap() const
Utility to print the mapping between the End Effector finger chains and the related actuated joints...
Definition: Parser.cpp:390
ros::NodeHandle _nh
Definition: Parser.h:171
int getActuatedJointsNumber() const
getter for the total number of actuated joints in the configuration files
Definition: Parser.cpp:420
std::string getUrdfString() const
get the whole urdf file parsed as a string
Definition: Parser.cpp:455
std::vector< int > _fingers_group_id
Definition: Parser.h:183
bool configure()
configure the ROSEE parser based on the configration files requested
Definition: Parser.cpp:326
bool _is_initialized
Definition: Parser.h:174
std::map< std::string, std::string > _joint_finger_map
Definition: Parser.h:188
bool init()
Initialization function using the ROS param ROSEEConfigPath.
Definition: Parser.cpp:362
std::string getEndEffectorName() const
getter for the configure End-Effector name
Definition: Parser.cpp:450
urdf::ModelInterfaceSharedPtr _urdf_model
Definition: Parser.h:176
virtual ~Parser()
Definition: Parser.cpp:28
int _joints_num
Definition: Parser.h:185
int _fingers_num
Definition: Parser.h:181
std::string getActionPath() const
get the path where emit and parse grasping actions
Definition: Parser.cpp:464
std::string _action_path
Definition: Parser.h:173
bool removePassiveJoints()
Function to remove the passive joints from the filled maps.
Definition: Parser.cpp:196
std::map< std::string, std::vector< std::string > > _finger_joint_map
Definition: Parser.h:187
bool parseSRDF()
Function responsible to parse the SRDF data.
Definition: Parser.cpp:85
std::string _srdf_path
Definition: Parser.h:172
void addNotInFingerJoints()
Definition: Parser.cpp:172
std::string _urdf_path
Definition: Parser.h:172
KDL::Tree _robot_tree
Definition: Parser.h:177
Parser(const ros::NodeHandle &nh)
Definition: Parser.cpp:22
std::map< std::string, urdf::JointConstSharedPtr > getUrdfJointMap() const
getter for the URDF information of the joints of the End-Effector
Definition: Parser.cpp:435
bool parseURDF()
Function responsible to get the file needed to fill the internal data structure of the Parser with da...
Definition: Parser.cpp:225
std::vector< std::string > _fingers_names
Definition: Parser.h:182
std::string _srdf_string
Definition: Parser.h:172
std::map< std::string, urdf::JointConstSharedPtr > _urdf_joint_map
Definition: Parser.h:189
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 ti...
Definition: Parser.cpp:32
std::map< std::string, std::string > getJointFingerMap() const
getter for a description of the End-Effector as a map of joint name, finger name
Definition: Parser.cpp:430
srdf::Model _srdfdom
Definition: Parser.h:179
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...
Definition: Parser.cpp:425
std::string _urdf_string
Definition: Parser.h:172