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 <end_effector/Parser.h>
19 :
20 : #define CHAIN_PER_GROUP 1
21 :
22 72 : ROSEE::Parser::Parser ( const rclcpp::Node::SharedPtr node ) :
23 72 : _node ( node ) {
24 :
25 72 : }
26 :
27 :
28 72 : ROSEE::Parser::~Parser() {
29 :
30 72 : }
31 :
32 190 : 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 190 : _finger_joint_map.insert(std::make_pair(finger_name, std::vector<std::string>()));
41 :
42 :
43 380 : KDL::Chain actual_chain;
44 190 : if ( _robot_tree.getChain ( base_link, tip_link, actual_chain ) ) {
45 :
46 190 : int segments_num = actual_chain.getNrOfSegments();
47 609 : for ( int i = 0; i < segments_num; i++ ) {
48 :
49 838 : KDL::Segment actual_segment = actual_chain.getSegment ( i );
50 838 : KDL::Joint actual_joint = actual_segment.getJoint();
51 :
52 419 : bool is_valid_joint = false;
53 :
54 : // Check if joint is revolute or prismatic
55 419 : is_valid_joint = actual_joint.getTypeName() == "RotAxis";
56 :
57 : // || actual_joint.getTypeName() == "TransAxis";
58 :
59 838 : auto urdf_joint = _urdf_model->getJoint(actual_joint.getName());
60 419 : 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 419 : if ( is_valid_joint && (!is_mimic_joint) ) {
65 :
66 380 : _finger_joint_map[finger_name].push_back ( actual_joint.getName() );
67 380 : _joint_finger_map[actual_joint.getName()] = finger_name;
68 380 : _urdf_joint_map[actual_joint.getName()] = _urdf_model->getJoint ( actual_joint.getName() );
69 :
70 380 : _joints_num++;
71 :
72 : }
73 : }
74 :
75 190 : return true;
76 : }
77 :
78 0 : RCLCPP_ERROR_STREAM (_node->get_logger(), "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 72 : bool ROSEE::Parser::parseSRDF() {
86 :
87 : // initialize srdfdom
88 72 : _srdfdom.initFile ( *_urdf_model, _srdf_path );
89 :
90 : // get the end-effectors in the SRDF file
91 144 : 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 72 : if ( srdf_end_effectors.size() != 1 ) {
96 :
97 0 : return false;
98 : }
99 :
100 72 : RCLCPP_INFO_STREAM (_node->get_logger(), "ros_end_effector Parser found end_effector: " << srdf_end_effectors.at ( 0 ).name_ );
101 :
102 : // get all the groups in the SRDF
103 144 : std::vector<srdf::Model::Group> srdf_groups = _srdfdom.getGroups();
104 :
105 : // TBD will be a vector
106 144 : srdf::Model::Group fingers_group;
107 : // find end effector fingers chains group
108 : // TBD will be a vector
109 72 : int group_num = srdf_groups.size();
110 : // TBD will be a vector
111 144 : std::string end_effector_group_name = srdf_end_effectors.at ( 0 ).component_group_;
112 334 : for ( int i = 0; i < group_num; i++ ) {
113 262 : if ( srdf_groups[i].name_ == end_effector_group_name ) {
114 72 : fingers_group = srdf_groups[i];
115 72 : RCLCPP_INFO_STREAM (_node->get_logger(), "ros_end_effector Parser found group: " << end_effector_group_name << " in the SRDF with the following fingers: " );
116 : }
117 : }
118 :
119 72 : _fingers_num = fingers_group.subgroups_.size();
120 72 : _fingers_names.resize ( _fingers_num );
121 72 : _fingers_group_id.resize ( _fingers_num );
122 :
123 : // fill the chain names vector
124 262 : for ( int i = 0; i < _fingers_num; i++ ) {
125 :
126 190 : _fingers_names[i] = fingers_group.subgroups_[i];
127 :
128 : // find the id of the current finger group
129 898 : for ( int j = 0; j < group_num; j++ ) {
130 708 : if ( srdf_groups[j].name_ == _fingers_names[i] ) {
131 190 : _fingers_group_id[i] = j;
132 : }
133 : }
134 190 : RCLCPP_INFO_STREAM (_node->get_logger(), _fingers_names[i] );
135 : }
136 :
137 : // iterate over the fingers and find revolute joints
138 262 : for ( int i = 0; i < _fingers_num; i++ ) {
139 190 : srdf::Model::Group current_finger_group = srdf_groups[ _fingers_group_id[i] ];
140 :
141 : // NOTE only one chain per group
142 190 : if ( current_finger_group.chains_.size() != CHAIN_PER_GROUP ) {
143 :
144 0 : RCLCPP_ERROR_STREAM (_node->get_logger(), "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 380 : if ( !getJointsInFinger ( current_finger_group.chains_[0].first,
151 190 : current_finger_group.chains_[0].second,
152 : current_finger_group.name_
153 : ) ) {
154 0 : return false;
155 : }
156 : }
157 :
158 72 : addNotInFingerJoints();
159 :
160 72 : removePassiveJoints();
161 :
162 : // save srdf as string
163 144 : std::ifstream t_srdf ( _srdf_path );
164 72 : std::stringstream buffer_srdf;
165 72 : buffer_srdf << t_srdf.rdbuf();
166 72 : _srdf_string = buffer_srdf.str();
167 :
168 72 : return true;
169 :
170 : }
171 :
172 72 : void ROSEE::Parser::addNotInFingerJoints() {
173 :
174 635 : for (auto it : _urdf_model->joints_) { //this contains all joints
175 :
176 563 : if (it.second->mimic == nullptr) { //not a mimic joint...
177 :
178 1048 : if (it.second->type == urdf::Joint::CONTINUOUS ||
179 524 : it.second->type == urdf::Joint::REVOLUTE ) {
180 : //it.second->type == urdf::Joint::PRISMATIC
181 :
182 380 : 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 72 : }
195 :
196 72 : bool ROSEE::Parser::removePassiveJoints() {
197 :
198 72 : std::vector<srdf::Model::PassiveJoint> passiveJointList = _srdfdom.getPassiveJoints();
199 :
200 72 : 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 144 : return true;
222 : }
223 :
224 :
225 72 : bool ROSEE::Parser::parseURDF() {
226 :
227 144 : std::string xml_string;
228 144 : std::fstream xml_file ( _urdf_path.c_str(), std::fstream::in );
229 72 : if ( xml_file.is_open() ) {
230 :
231 18085 : while ( xml_file.good() ) {
232 :
233 18013 : std::string line;
234 18013 : std::getline ( xml_file, line );
235 18013 : xml_string += ( line + "\n" );
236 : }
237 :
238 72 : xml_file.close();
239 72 : _urdf_model = urdf::parseURDF ( xml_string );
240 :
241 : // create the robot KDL tree from the URDF model
242 72 : if ( !kdl_parser::treeFromUrdfModel ( *_urdf_model, _robot_tree ) ) {
243 :
244 0 : RCLCPP_ERROR_STREAM (_node->get_logger(), "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 144 : std::ifstream t_urdf ( _urdf_path );
250 72 : std::stringstream buffer_urdf;
251 72 : buffer_urdf << t_urdf.rdbuf();
252 72 : _urdf_string = buffer_urdf.str();
253 :
254 72 : return true;
255 :
256 : } else {
257 :
258 0 : RCLCPP_ERROR_STREAM (_node->get_logger(), "in " << __func__ << " : Can NOT open " << _urdf_path << " !" );
259 0 : return false;
260 : }
261 : }
262 :
263 :
264 72 : bool ROSEE::Parser::configure() {
265 :
266 72 : bool ret = true;
267 :
268 72 : if ( parseURDF() ) {
269 :
270 72 : if ( parseSRDF() ) {
271 :
272 72 : RCLCPP_INFO_STREAM (_node->get_logger(), "ROSEndEffector Parser successfully configured using urdf file: " << _urdf_path
273 : << "\n\t srdf file: " << _srdf_path << "\n\t actions folder " << _action_path
274 : );
275 :
276 : } else {
277 :
278 0 : RCLCPP_ERROR_STREAM (_node->get_logger(), "ROSEndEffector Parser error while parsing SRDF");
279 0 : ret = false;
280 : }
281 :
282 : } else {
283 :
284 0 : RCLCPP_ERROR_STREAM (_node->get_logger(), "ROSEndEffector Parser error while parsing URDF");
285 0 : ret = false;
286 : }
287 :
288 :
289 :
290 :
291 72 : return ret;
292 : }
293 :
294 :
295 :
296 0 : bool ROSEE::Parser::init() {
297 :
298 : // in ROS2, param must be declare first to be find
299 0 : _node->declare_parameter ( "urdf_path", "" );
300 0 : _node->declare_parameter ( "srdf_path", "" );
301 0 : _node->declare_parameter ( "actions_folder_path", "" );
302 :
303 0 : if ( _node->get_parameter ( "urdf_path", _urdf_path ) &&
304 0 : _node->get_parameter ( "srdf_path", _srdf_path ) &&
305 0 : _node->get_parameter ( "actions_folder_path", _action_path )
306 : ) {
307 :
308 : //_action_path = std::string(getenv("HOME")) + "/" + _action_path;
309 :
310 0 : _is_initialized = configure();
311 0 : return _is_initialized;
312 : }
313 :
314 : // error
315 0 : RCLCPP_ERROR_STREAM (_node->get_logger(), "in " << __func__ << " : '_urdf_path' and/or '_srdf_path' and/or 'actions_folder_path' not found on ROS parameter server" );
316 0 : return false;
317 :
318 : }
319 :
320 :
321 72 : bool ROSEE::Parser::init ( const std::string& urdf_path, const std::string& srdf_path, const std::string& action_path ) {
322 :
323 72 : _urdf_path = urdf_path;
324 72 : _srdf_path = srdf_path;
325 72 : _action_path = action_path;
326 :
327 72 : _is_initialized = configure();
328 72 : return _is_initialized;
329 : }
330 :
331 :
332 20 : void ROSEE::Parser::printEndEffectorFingerJointsMap() const {
333 :
334 20 : if ( _is_initialized ) {
335 :
336 20 : RCLCPP_INFO_STREAM (_node->get_logger(), "ROS End Effector: Finger Joints Map" );
337 20 : RCLCPP_INFO_STREAM (_node->get_logger(), "-------------------------" );
338 80 : for ( auto& chain_joints: _finger_joint_map ) {
339 60 : RCLCPP_INFO_STREAM (_node->get_logger(), chain_joints.first );
340 :
341 60 : int nJointInFinger = chain_joints.second.size();
342 :
343 60 : if ( nJointInFinger == 0 ) {
344 :
345 0 : RCLCPP_INFO_STREAM (_node->get_logger(), "No actuated joint in this finger" );
346 :
347 : } else {
348 :
349 180 : for ( int i = 0; i < nJointInFinger ; i++ ) {
350 120 : RCLCPP_INFO_STREAM (_node->get_logger(), chain_joints.second.at ( i ) );
351 : }
352 : }
353 :
354 60 : RCLCPP_INFO_STREAM (_node->get_logger(), "-------------------------" );
355 : }
356 : } else {
357 :
358 0 : RCLCPP_ERROR_STREAM (_node->get_logger(), "in " << __func__ << " : ROSEE::Parser needs to be initialized. Call init() frist." );
359 : }
360 20 : }
361 :
362 72 : int ROSEE::Parser::getActuatedJointsNumber() const {
363 :
364 72 : return _joints_num;
365 : }
366 :
367 72 : std::map< std::string, std::vector< std::string >> ROSEE::Parser::getFingerJointMap() const {
368 :
369 72 : return _finger_joint_map;
370 : }
371 :
372 0 : std::map< std::string, std::string > ROSEE::Parser::getJointFingerMap() const {
373 :
374 0 : return _joint_finger_map;
375 : }
376 :
377 72 : std::map< std::string, urdf::JointConstSharedPtr > ROSEE::Parser::getUrdfJointMap() const {
378 :
379 72 : return _urdf_joint_map;
380 : }
381 :
382 0 : void ROSEE::Parser::getFingerJointMap( std::map< std::string, std::vector< std::string >>& finger_joint_map ) const {
383 :
384 0 : finger_joint_map = _finger_joint_map;
385 0 : }
386 :
387 0 : void ROSEE::Parser::getJointFingerMap ( std::map< std::string, std::string >& joint_finger_map ) const {
388 :
389 0 : joint_finger_map = _joint_finger_map;
390 0 : }
391 :
392 72 : std::string ROSEE::Parser::getEndEffectorName() const {
393 :
394 72 : return _urdf_model->getName();
395 : }
396 :
397 0 : std::string ROSEE::Parser::getUrdfString() const {
398 0 : return _urdf_string;
399 : }
400 :
401 0 : std::string ROSEE::Parser::getSrdfString() const {
402 0 : return _srdf_string;
403 : }
404 :
405 :
406 52 : std::string ROSEE::Parser::getActionPath() const {
407 52 : return _action_path;
408 : }
409 :
410 :
411 :
412 :
|
