Line data Source code
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 <ros_end_effector/Parser.h>
19 :
20 : #define CHAIN_PER_GROUP 1
21 :
22 113 : ROSEE::Parser::Parser ( const ros::NodeHandle& nh ) :
23 113 : _nh ( nh ) {
24 :
25 113 : }
26 :
27 :
28 113 : ROSEE::Parser::~Parser() {
29 :
30 113 : }
31 :
32 294 : 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 294 : _finger_joint_map.insert(std::make_pair(finger_name, std::vector<std::string>()));
41 :
42 :
43 588 : KDL::Chain actual_chain;
44 294 : if ( _robot_tree.getChain ( base_link, tip_link, actual_chain ) ) {
45 :
46 294 : int segments_num = actual_chain.getNrOfSegments();
47 957 : for ( int i = 0; i < segments_num; i++ ) {
48 :
49 1326 : KDL::Segment actual_segment = actual_chain.getSegment ( i );
50 1326 : KDL::Joint actual_joint = actual_segment.getJoint();
51 :
52 663 : bool is_valid_joint = false;
53 :
54 : // Check if joint is revolute or prismatic
55 663 : is_valid_joint = actual_joint.getTypeName() == "RotAxis";
56 :
57 : // || actual_joint.getTypeName() == "TransAxis";
58 :
59 1326 : auto urdf_joint = _urdf_model->getJoint(actual_joint.getName());
60 663 : 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 663 : if ( is_valid_joint && (!is_mimic_joint) ) {
65 :
66 600 : _finger_joint_map[finger_name].push_back ( actual_joint.getName() );
67 600 : _joint_finger_map[actual_joint.getName()] = finger_name;
68 600 : _urdf_joint_map[actual_joint.getName()] = _urdf_model->getJoint ( actual_joint.getName() );
69 :
70 600 : _joints_num++;
71 :
72 : }
73 : }
74 :
75 294 : return true;
76 : }
77 :
78 0 : ROS_ERROR_STREAM ( "chain from base_link " << base_link << " to tip_link " << tip_link << " not found in the URDF" );
79 :
80 0 : return false;
81 : }
82 :
83 :
84 :
85 113 : bool ROSEE::Parser::parseSRDF() {
86 :
87 : // initialize srdfdom
88 113 : _srdfdom.initFile ( *_urdf_model, _srdf_path );
89 :
90 : // get the end-effectors in the SRDF file
91 226 : 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 113 : if ( srdf_end_effectors.size() != 1 ) {
96 :
97 0 : return false;
98 : }
99 :
100 113 : 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 226 : std::vector<srdf::Model::Group> srdf_groups = _srdfdom.getGroups();
104 :
105 : // TBD will be a vector
106 226 : srdf::Model::Group fingers_group;
107 : // find end effector fingers chains group
108 : // TBD will be a vector
109 113 : int group_num = srdf_groups.size();
110 : // TBD will be a vector
111 226 : std::string end_effector_group_name = srdf_end_effectors.at ( 0 ).component_group_;
112 520 : for ( int i = 0; i < group_num; i++ ) {
113 407 : if ( srdf_groups[i].name_ == end_effector_group_name ) {
114 113 : fingers_group = srdf_groups[i];
115 113 : ROS_INFO_STREAM ( "ros_end_effector Parser found group: " << end_effector_group_name << " in the SRDF with the following fingers: " );
116 : }
117 : }
118 :
119 113 : _fingers_num = fingers_group.subgroups_.size();
120 113 : _fingers_names.resize ( _fingers_num );
121 113 : _fingers_group_id.resize ( _fingers_num );
122 :
123 : // fill the chain names vector
124 407 : for ( int i = 0; i < _fingers_num; i++ ) {
125 :
126 294 : _fingers_names[i] = fingers_group.subgroups_[i];
127 :
128 : // find the id of the current finger group
129 1380 : for ( int j = 0; j < group_num; j++ ) {
130 1086 : if ( srdf_groups[j].name_ == _fingers_names[i] ) {
131 294 : _fingers_group_id[i] = j;
132 : }
133 : }
134 294 : ROS_INFO_STREAM ( _fingers_names[i] );
135 : }
136 :
137 : // iterate over the fingers and find revolute joints
138 407 : for ( int i = 0; i < _fingers_num; i++ ) {
139 588 : srdf::Model::Group current_finger_group = srdf_groups[ _fingers_group_id[i] ];
140 :
141 : // NOTE only one chain per group
142 294 : if ( current_finger_group.chains_.size() != CHAIN_PER_GROUP ) {
143 :
144 0 : 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 0 : return false;
147 : }
148 :
149 : // fill the enabled/disabled joints in chain map
150 588 : if ( !getJointsInFinger ( current_finger_group.chains_[0].first,
151 294 : current_finger_group.chains_[0].second,
152 : current_finger_group.name_
153 : ) ) {
154 0 : return false;
155 : }
156 : }
157 :
158 113 : addNotInFingerJoints();
159 :
160 113 : removePassiveJoints();
161 :
162 : // save srdf as string
163 226 : std::ifstream t_srdf ( _srdf_path );
164 226 : std::stringstream buffer_srdf;
165 113 : buffer_srdf << t_srdf.rdbuf();
166 113 : _srdf_string = buffer_srdf.str();
167 :
168 113 : return true;
169 :
170 : }
171 :
172 113 : void ROSEE::Parser::addNotInFingerJoints() {
173 :
174 1002 : for (auto it : _urdf_model->joints_) { //this contains all joints
175 :
176 889 : if (it.second->mimic == nullptr) { //not a mimic joint...
177 :
178 1652 : if (it.second->type == urdf::Joint::CONTINUOUS ||
179 826 : it.second->type == urdf::Joint::REVOLUTE ) {
180 : //it.second->type == urdf::Joint::PRISMATIC
181 :
182 600 : if (_urdf_joint_map.find(it.second->name) == _urdf_joint_map.end() ) {
183 :
184 0 : _urdf_joint_map.insert(std::make_pair(it.second->name, it.second));
185 0 : _finger_joint_map["virtual_finger"].push_back ( it.second->name );
186 0 : _joint_finger_map[it.second->name] = "virtual_finger";
187 0 : _joints_num++;
188 : }
189 : }
190 : }
191 :
192 : }
193 :
194 113 : }
195 :
196 113 : bool ROSEE::Parser::removePassiveJoints() {
197 :
198 226 : std::vector<srdf::Model::PassiveJoint> passiveJointList = _srdfdom.getPassiveJoints();
199 :
200 113 : 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 0 : if (_urdf_joint_map.erase(passiveJoint.name_) > 0) {
203 :
204 : //we have also to remove it from the other maps
205 0 : std::string fingerOfPassive = _joint_finger_map.at(passiveJoint.name_);
206 0 : _joint_finger_map.erase(passiveJoint.name_);
207 :
208 0 : for (auto it = _finger_joint_map.at(fingerOfPassive).begin();
209 0 : it!= _finger_joint_map.at(fingerOfPassive).end(); ++it){
210 :
211 0 : if (it->compare(passiveJoint.name_) == 0 ) {
212 0 : _finger_joint_map.at(fingerOfPassive).erase(it);
213 0 : break;
214 : }
215 : }
216 :
217 0 : _joints_num--;
218 : }
219 : }
220 :
221 226 : return true;
222 : }
223 :
224 :
225 113 : bool ROSEE::Parser::parseURDF() {
226 :
227 226 : std::string xml_string;
228 226 : std::fstream xml_file ( _urdf_path.c_str(), std::fstream::in );
229 113 : if ( xml_file.is_open() ) {
230 :
231 56449 : while ( xml_file.good() ) {
232 :
233 56336 : std::string line;
234 28168 : std::getline ( xml_file, line );
235 28168 : xml_string += ( line + "\n" );
236 : }
237 :
238 113 : xml_file.close();
239 113 : _urdf_model = urdf::parseURDF ( xml_string );
240 :
241 : // create the robot KDL tree from the URDF model
242 113 : if ( !kdl_parser::treeFromUrdfModel ( *_urdf_model, _robot_tree ) ) {
243 :
244 0 : ROS_ERROR_STREAM ( "in " << __func__ << " Failed to construct kdl tree" );
245 0 : return false;
246 : }
247 :
248 : // save urdf and string (can be useful to have, thanks @alaurenzi!)
249 226 : std::ifstream t_urdf ( _urdf_path );
250 226 : std::stringstream buffer_urdf;
251 113 : buffer_urdf << t_urdf.rdbuf();
252 113 : _urdf_string = buffer_urdf.str();
253 :
254 113 : return true;
255 :
256 : } else {
257 :
258 0 : ROS_ERROR_STREAM ( "in " << __func__ << " : Can NOT open " << _urdf_path << " !" );
259 0 : return false;
260 : }
261 : }
262 :
263 :
264 113 : bool ROSEE::Parser::getROSEndEffectorConfig() {
265 :
266 113 : bool success = true;
267 :
268 : // check if the ROSEE config path exists
269 226 : std::ifstream fin ( _ros_ee_config_path );
270 113 : if ( fin.fail() ) {
271 :
272 0 : ROS_ERROR_STREAM ( "in " << __func__ << " : Can NOT open " << _ros_ee_config_path << "!" );
273 0 : success = false;
274 : } else {
275 :
276 : // load the node for the _ros_ee_config_path
277 226 : YAML::Node cfg = YAML::LoadFile ( _ros_ee_config_path );
278 :
279 : // find the internal node ros_end_effector
280 226 : YAML::Node ros_ee_node;
281 113 : if ( cfg["ros_end_effector"] ) {
282 :
283 113 : ros_ee_node = cfg["ros_end_effector"];
284 : } else {
285 :
286 0 : ROS_ERROR_STREAM ( "in " << __func__ << " : YAML file " << _ros_ee_config_path << " does not contain ros_end_effector mandatory node!!" );
287 0 : success = false;
288 : }
289 :
290 : // check the urdf_filename
291 113 : if ( ros_ee_node["urdf_path"] ) {
292 :
293 : // TBD relative path in more elegant way
294 113 : _urdf_path = ROSEE::Utils::getPackagePath() + "/configs/" + ros_ee_node["urdf_path"].as<std::string>();
295 113 : ROS_INFO_STREAM ( "ros_end_effector Parser found URDF path: " << _urdf_path );
296 : } else {
297 :
298 0 : ROS_ERROR_STREAM ( "in " << __func__ << " : ros_end_effector node of " << _ros_ee_config_path << " does not contain urdf_path mandatory node!!" );
299 0 : success = false;
300 : }
301 :
302 : // check the srdf_filename
303 113 : if ( ros_ee_node["srdf_path"] ) {
304 :
305 : // TBD relative path in more elegant way
306 113 : _srdf_path = ROSEE::Utils::getPackagePath() + "/configs/" + ros_ee_node["srdf_path"].as<std::string>();
307 113 : ROS_INFO_STREAM ( "ros_end_effector Parser found SRDF path: " << _srdf_path );
308 : } else {
309 :
310 0 : ROS_ERROR_STREAM ( "in " << __func__ << " : ros_end_effector node of " << _ros_ee_config_path << " does not contain srdf_path mandatory node!!" );
311 0 : success = false;
312 : }
313 :
314 113 : if ( ros_ee_node["action_path"] ) {
315 113 : _action_path = ROSEE::Utils::getPackagePath() + "/configs/" + ros_ee_node["action_path"].as<std::string>();
316 113 : ROS_INFO_STREAM ( "ros_end_effector Parser found config folder for actions: " << _action_path );
317 :
318 : }
319 : }
320 :
321 226 : return success;
322 : }
323 :
324 113 : bool ROSEE::Parser::configure() {
325 :
326 113 : bool ret = true;
327 113 : if ( getROSEndEffectorConfig() ) {
328 :
329 113 : if ( parseURDF() ) {
330 :
331 113 : if ( parseSRDF() ) {
332 :
333 113 : ROS_INFO_STREAM ( "ROSEndEffector Parser successfully configured using config file: " << _ros_ee_config_path );
334 :
335 : } else {
336 :
337 0 : ROS_ERROR_STREAM ( "ROSEndEffector Parser error while parsing SRDF");
338 0 : ret = false;
339 : }
340 :
341 : } else {
342 :
343 0 : ROS_ERROR_STREAM ( "ROSEndEffector Parser error while parsing URDF");
344 0 : ret = false;
345 : }
346 :
347 : } else {
348 :
349 0 : ret = false;
350 : }
351 :
352 :
353 113 : return ret;
354 : }
355 :
356 :
357 :
358 41 : bool ROSEE::Parser::init() {
359 :
360 : // try to retrive the path to config from the ROS param server TBD namespace should be take into account
361 41 : if ( _nh.getParam ( "/ros_ee_config_path", _ros_ee_config_path ) ) {
362 :
363 41 : _is_initialized = configure();
364 41 : return _is_initialized;
365 : }
366 :
367 : // error
368 0 : ROS_ERROR_STREAM ( "in " << __func__ << " : ros_ee_config_path not found on ROS parameter server" );
369 0 : return false;
370 :
371 : }
372 :
373 72 : bool ROSEE::Parser::init ( const std::string& path_to_cfg ) {
374 :
375 72 : _ros_ee_config_path = path_to_cfg;
376 :
377 72 : _is_initialized = configure();
378 72 : return _is_initialized;
379 : }
380 :
381 61 : void ROSEE::Parser::printEndEffectorFingerJointsMap() const {
382 :
383 61 : if ( _is_initialized ) {
384 :
385 61 : ROS_INFO_STREAM ( "ROS End Effector: Finger Joints Map" );
386 61 : ROS_INFO_STREAM ( "-------------------------" );
387 225 : for ( auto& chain_joints: _finger_joint_map ) {
388 164 : ROS_INFO_STREAM ( chain_joints.first );
389 :
390 164 : int nJointInFinger = chain_joints.second.size();
391 :
392 164 : if ( nJointInFinger == 0 ) {
393 :
394 8 : ROS_INFO_STREAM ( "No actuated joint in this finger" );
395 :
396 : } else {
397 :
398 496 : for ( int i = 0; i < nJointInFinger ; i++ ) {
399 340 : ROS_INFO_STREAM ( chain_joints.second.at ( i ) );
400 : }
401 : }
402 :
403 164 : ROS_INFO_STREAM ( "-------------------------" );
404 : }
405 : } else {
406 :
407 0 : ROS_ERROR_STREAM ( "in " << __func__ << " : ROSEE::Parser needs to be initialized. Call init() frist." );
408 : }
409 61 : }
410 :
411 113 : int ROSEE::Parser::getActuatedJointsNumber() const {
412 :
413 113 : return _joints_num;
414 : }
415 :
416 113 : std::map< std::string, std::vector< std::string >> ROSEE::Parser::getFingerJointMap() const {
417 :
418 113 : return _finger_joint_map;
419 : }
420 :
421 0 : std::map< std::string, std::string > ROSEE::Parser::getJointFingerMap() const {
422 :
423 0 : return _joint_finger_map;
424 : }
425 :
426 113 : std::map< std::string, urdf::JointConstSharedPtr > ROSEE::Parser::getUrdfJointMap() const {
427 :
428 113 : return _urdf_joint_map;
429 : }
430 :
431 0 : void ROSEE::Parser::getFingerJointMap( std::map< std::string, std::vector< std::string >>& finger_joint_map ) const {
432 :
433 0 : finger_joint_map = _finger_joint_map;
434 0 : }
435 :
436 0 : void ROSEE::Parser::getJointFingerMap ( std::map< std::string, std::string >& joint_finger_map ) const {
437 :
438 0 : joint_finger_map = _joint_finger_map;
439 0 : }
440 :
441 113 : std::string ROSEE::Parser::getEndEffectorName() const {
442 :
443 113 : return _urdf_model->getName();
444 : }
445 :
446 0 : std::string ROSEE::Parser::getUrdfString() const {
447 0 : return _urdf_string;
448 : }
449 :
450 0 : std::string ROSEE::Parser::getSrdfString() const {
451 0 : return _srdf_string;
452 : }
453 :
454 0 : std::string ROSEE::Parser::getRoseeConfigPath() const {
455 0 : return _ros_ee_config_path;
456 : }
457 :
458 93 : std::string ROSEE::Parser::getActionPath() const {
459 93 : return _action_path;
460 147 : };
461 :
462 :
463 :
464 :
|
