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/UniversalRosEndEffectorExecutor.h>
19 :
20 :
21 41 : ROSEE::UniversalRosEndEffectorExecutor::UniversalRosEndEffectorExecutor ( std::string ns ) : _nh ( ns ) {
22 :
23 41 : if ( ! _nh.getParam ( "/rate", _rate ) ) {
24 0 : ROS_INFO_STREAM ( "Ros parameter for rate not found, I'm setting the default rate of 100 Hz" );
25 0 : _rate = 100.0;
26 : }
27 41 : _period = 1.0 / _rate;
28 82 : _loop_timer = _nh.createTimer ( ros::Duration ( _period ),
29 : &UniversalRosEndEffectorExecutor::timer_callback,
30 41 : this, false, false );
31 41 : _time = 0.0;
32 :
33 82 : ROSEE::Parser p ( _nh );
34 41 : p.init (); //TBD check return
35 41 : p.printEndEffectorFingerJointsMap();
36 :
37 : // retrieve the ee interface
38 41 : _ee = std::make_shared<ROSEE::EEInterface> ( p );
39 41 : ROS_INFO_STREAM ( "Fingers in EEInterface: " );
40 145 : for ( auto& f : _ee->getFingers() ) {
41 104 : ROS_INFO_STREAM ( f );
42 : }
43 :
44 41 : folderForActions = p.getActionPath();
45 41 : if ( folderForActions.size() == 0 ){ //if no action path is set in the yaml file...
46 0 : folderForActions = ROSEE::Utils::getPackagePath() + "/configs/actions/" + _ee->getName();
47 : }
48 :
49 41 : _motors_names =_ee->getActuatedJoints();
50 :
51 : // prepare publisher which publish motor references
52 41 : init_motor_reference_pub();
53 :
54 : //init q_ref and the filter
55 41 : init_qref_filter();
56 :
57 : // primitives
58 41 : init_grasping_primitive();
59 :
60 : //services, check on mapActionHandlerPtr is done inside the costructor
61 41 : _ros_service_handler = std::make_shared<RosServiceHandler>(&_nh, mapActionHandlerPtr, folderForActions+"/generics/");
62 41 : _ros_service_handler->init(_ee->getFingers().size());
63 :
64 : // actions
65 82 : std::string actionGraspingCommandName;
66 41 : _nh.param<std::string>("/rosee/rosAction_grasping_command", actionGraspingCommandName, "action_command");
67 41 : _ros_action_server = std::make_shared<RosActionServer> (actionGraspingCommandName , &_nh);
68 41 : timed_requested = false;
69 41 : timed_index = -1;
70 :
71 41 : init_joint_state_sub();
72 :
73 41 : }
74 :
75 41 : bool ROSEE::UniversalRosEndEffectorExecutor::init_motor_reference_pub() {
76 :
77 : //Fixed topic, Hal will read always from here
78 82 : std::string motor_reference_topic = "motor_reference_pos";
79 41 : const int motor_reference_queue = 10;
80 :
81 : //We could use MotorPosition message... but with JointState the hal node has not to include
82 : //our rosee_msg package, so maybe it is better use official ros JointState
83 41 : _motor_reference_pub = _nh.advertise<sensor_msgs::JointState> ( motor_reference_topic, motor_reference_queue );
84 41 : _motors_num = _motors_names.size();
85 : //mr = motor reference
86 41 : _mr_msg.name = _motors_names;
87 41 : _mr_msg.position.resize ( _motors_num );
88 41 : _mr_msg.velocity.resize ( _motors_num );
89 41 : _mr_msg.effort.resize ( _motors_num );
90 :
91 82 : return true;
92 : }
93 :
94 41 : bool ROSEE::UniversalRosEndEffectorExecutor::init_qref_filter() {
95 :
96 : // filter TBD select filter profile
97 41 : const double DAMPING_FACT = 1.0;
98 41 : const double BW_MEDIUM = 2.0;
99 41 : double omega = 2.0 * M_PI * BW_MEDIUM;
100 :
101 41 : _filt_q.setDamping ( DAMPING_FACT );
102 41 : _filt_q.setTimeStep ( _period );
103 41 : _filt_q.setOmega ( omega );
104 :
105 : // initialize references
106 41 : _qref.resize ( _motors_num );
107 41 : _qref_optional.resize ( _motors_num );
108 41 : _qref_optional.setZero();
109 : // TBD init from current position reference
110 41 : _qref.setZero();
111 : // reset filter
112 41 : _filt_q.reset ( _qref );
113 :
114 41 : return true;
115 : }
116 :
117 :
118 41 : bool ROSEE::UniversalRosEndEffectorExecutor::init_grasping_primitive() {
119 :
120 : // get all action in the handler
121 41 : mapActionHandlerPtr = std::make_shared<ROSEE::MapActionHandler>();
122 41 : mapActionHandlerPtr->parseAllActions(folderForActions);
123 :
124 : // pinch tight
125 41 : _pinchParsedMap = mapActionHandlerPtr->getPrimitiveMap("pinchTight");
126 :
127 : // pinch loose
128 41 : if (mapActionHandlerPtr->getPrimitiveMap(ROSEE::ActionPrimitive::Type::PinchLoose).size()>0) {
129 : //another method to get the map
130 8 : _pinchLooseParsedMap = mapActionHandlerPtr->getPrimitiveMap(ROSEE::ActionPrimitive::Type::PinchLoose).at(0);
131 : }
132 :
133 : // trig, tip flex and fing flex
134 41 : _trigParsedMap = mapActionHandlerPtr->getPrimitiveMap("trig");
135 41 : _tipFlexParsedMap = mapActionHandlerPtr->getPrimitiveMap("tipFlex");
136 41 : _fingFlexParsedMap = mapActionHandlerPtr->getPrimitiveMap("fingFlex");
137 :
138 : // NOTE maps useful just to recap
139 41 : ROS_INFO_STREAM ( "PINCHES-TIGHT:" );
140 96 : for ( auto &i : _pinchParsedMap ) {
141 55 : i.second->print();
142 : }
143 41 : ROS_INFO_STREAM ( "PINCHES-LOOSE:" );
144 49 : for ( auto &i : _pinchLooseParsedMap ) {
145 8 : i.second->print();
146 : }
147 41 : ROS_INFO_STREAM ( "TRIGGERS:" );
148 129 : for ( auto &i : _trigParsedMap ) {
149 88 : i.second->print();
150 : }
151 41 : ROS_INFO_STREAM ( "TIP FLEX:" );
152 129 : for ( auto &i : _tipFlexParsedMap ) {
153 88 : i.second->print();
154 : }
155 41 : ROS_INFO_STREAM ( "FINGER FLEX:" );
156 129 : for ( auto &i : _fingFlexParsedMap ) {
157 88 : i.second->print();
158 : }
159 :
160 : // composed actions
161 41 : _graspParsed = mapActionHandlerPtr->getGeneric("grasp");
162 :
163 : // recap
164 41 : ROS_INFO_STREAM ( "GRASP:" );
165 41 : if (_graspParsed != nullptr) {
166 33 : _graspParsed->print();
167 : }
168 :
169 41 : return true;
170 : }
171 :
172 41 : void ROSEE::UniversalRosEndEffectorExecutor::init_joint_state_sub () {
173 :
174 : //Fixed topic, Hal will publish always here
175 82 : std::string topic_name_js = "/ros_end_effector/joint_states";
176 :
177 41 : ROS_INFO_STREAM ( "Getting joint pos from '" << topic_name_js << "'" );
178 :
179 82 : _joint_state_sub = _nh.subscribe (topic_name_js, 1,
180 41 : &ROSEE::UniversalRosEndEffectorExecutor::joint_state_clbk, this);
181 41 : }
182 :
183 20762 : void ROSEE::UniversalRosEndEffectorExecutor::joint_state_clbk(const sensor_msgs::JointStateConstPtr& msg) {
184 :
185 : //We store all what we receive from hal node, to not loose time in the clbk to not consider the not motors
186 139033 : for (int i=0; i< msg->name.size(); i++) {
187 236542 : std::vector <double> one_dof { msg->position.at(i) };
188 118271 : _joint_actual_pos[msg->name.at(i)] = one_dof;
189 : }
190 20762 : }
191 :
192 :
193 : // set q ref
194 33 : bool ROSEE::UniversalRosEndEffectorExecutor::updateGoal() {
195 :
196 66 : rosee_msg::ROSEEActionControl goal = _ros_action_server->getGoal();
197 :
198 33 : timed_requested = false;
199 :
200 33 : if (goal.action_type != ROSEE::Action::Type::Timed){
201 29 : if (goal.percentage < 0 || goal.percentage > 1) {
202 0 : ROS_ERROR_STREAM ( "Received an action-goal with percentage " << goal.percentage <<
203 : " Please insert a value between 0 (for 0%) and 1 (for 100%) ");
204 0 : _ros_action_server->abortGoal("Percentage not valid");
205 0 : return false;
206 :
207 : }
208 : }
209 :
210 : // we need this as global member because in send_feedback we need it...
211 : //JointsInvolvedCount joint_involved_mask;
212 :
213 : //Be sure that action_type of goal is defined well in msg, it is not a real enum
214 33 : switch (goal.action_type) {
215 :
216 14 : case ROSEE::Action::Type::Primitive :
217 : {
218 14 : ROSEE::ActionPrimitive::Ptr primitive = mapActionHandlerPtr->getPrimitive (goal.action_name, goal.selectable_items);
219 :
220 14 : if (primitive == nullptr) {
221 : //error message already printed in getPrimitive
222 0 : _ros_action_server->abortGoal("Primitive Action not found");
223 0 : return false;
224 : }
225 :
226 14 : _motor_position_goal = primitive->getJointPos();
227 14 : _motor_involved_mask = primitive->getJointsInvolvedCount();
228 :
229 14 : break;
230 : }
231 15 : case ROSEE::Action::Type::Generic : // same thing as composed
232 : case ROSEE::Action::Type::Composed :
233 : {
234 15 : ROSEE::ActionGeneric::Ptr generic = mapActionHandlerPtr->getGeneric(goal.action_name);
235 :
236 15 : if (generic == nullptr) {
237 : //error message already printed in getGeneric
238 0 : _ros_action_server->abortGoal("Generic Action not found");
239 0 : return false;
240 : }
241 :
242 15 : _motor_position_goal = generic->getJointPos();
243 15 : _motor_involved_mask = generic->getJointsInvolvedCount();
244 :
245 15 : break;
246 : }
247 4 : case ROSEE::Action::Type::Timed : {
248 : // here we take the first of the timed action and we set refs for it
249 : //TODO first time margin (before) is not considered?
250 :
251 4 : timedAction = mapActionHandlerPtr->getTimed(goal.action_name);
252 :
253 4 : if (timedAction == nullptr) {
254 : //error message already printed in getTimed
255 0 : _ros_action_server->abortGoal("Timed Action not found");
256 0 : return false;
257 : }
258 :
259 4 : timed_requested = true;
260 4 : timed_index = 0;
261 4 : _motor_position_goal = timedAction->getAllJointPos().at(0);
262 12 : _motor_involved_mask = timedAction->getJointCountAction(
263 12 : timedAction->getInnerActionsNames().at(0));
264 :
265 4 : goal.percentage = 1; //so we are sure it is set, for timed is always 100%
266 :
267 4 : break;
268 : }
269 0 : case ROSEE::Action::Type::None :
270 : {
271 0 : ROS_ERROR_STREAM ( "Received an action-goal of type None (" << goal.action_type <<
272 : ") which is a no-type. Please use valid Action::Type ");
273 0 : _ros_action_server->abortGoal("Invalid type NONE");
274 0 : return false;
275 : break;
276 : }
277 0 : default : {
278 0 : ROS_ERROR_STREAM ( "Received an action-goal of type " << goal.action_type <<
279 : " which I do not recognize. Please use valid Action::Type ");
280 0 : _ros_action_server->abortGoal("Invalid type " + std::to_string(goal.action_type) );
281 0 : return false;
282 : }
283 : }
284 :
285 : //WE reset this so if it is not used it stay to zero
286 33 : _qref_optional.setZero();
287 33 : if (goal.optional_motors_names.size() != 0) {
288 0 : readOptionalCommands(goal.optional_motors_names, goal.optional_motors_commands);
289 : }
290 :
291 33 : return updateRefGoal(goal.percentage);
292 :
293 : }
294 :
295 0 : bool ROSEE::UniversalRosEndEffectorExecutor::readOptionalCommands(
296 : std::vector<std::string> motors_names, std::vector<double> motors_commands) {
297 :
298 0 : bool flag = true;
299 :
300 0 : if (motors_names.size() != motors_commands.size() &&
301 0 : motors_names.size() != _motors_num) {
302 0 : ROS_ERROR_STREAM ( "In receiving the goal command, the optional field is formed badly: "
303 : << "optional_motors_names and optional_motors_commands and number of motors have different size ("
304 : << motors_names.size() << " and " << motors_commands.size() << " and " << _motors_num
305 : << " respectively). I will ignore the optional command");
306 0 : return false;
307 : }
308 :
309 :
310 0 : for (int i=0; i<motors_names.size(); i++) {
311 :
312 0 : int id =-1;
313 0 : _ee->getInternalIdForJoint ( motors_names.at(i), id );
314 0 : if( id >= 0 ) {
315 : // NOTE assume single dof
316 0 : _qref_optional[id] = motors_commands.at(i);
317 : }
318 :
319 : else {
320 0 : ROS_WARN_STREAM ( "Trying to send an optional command to motor: " << motors_names.at(i)
321 : << " which is not defined" );
322 0 : flag = false;
323 : }
324 : }
325 :
326 : //TODO we have to filter also qref_optional ??
327 :
328 0 : return flag;
329 : }
330 :
331 38 : bool ROSEE::UniversalRosEndEffectorExecutor::updateRefGoal(double percentage) {
332 :
333 38 : normGoalFromInitialPos = 0;
334 250 : for ( auto it : _motor_involved_mask ) {
335 :
336 212 : if ( it.second != 0 ) {
337 134 : int id = -1;
338 134 : _ee->getInternalIdForJoint ( it.first, id );
339 :
340 134 : if( id >= 0 ) {
341 : // NOTE assume single joint
342 134 : _qref[id] = _motor_position_goal.at ( it.first ).at ( 0 ) * percentage;
343 : // to give the % as feedback, we store the initial distance from the goal
344 : //TODO take care that initially jointPos can be empty...
345 134 : normGoalFromInitialPos += pow (_qref[id] - _joint_actual_pos.at(it.first).at(0), 2 ) ;
346 : }
347 : else {
348 0 : ROS_WARN_STREAM ( "Trying to move Joint: " << it.first << " with ID: " << id );
349 0 : _ros_action_server->abortGoal("Invalid Joint id" );
350 0 : return false;
351 : }
352 : }
353 : }
354 38 : normGoalFromInitialPos = sqrt(normGoalFromInitialPos);
355 :
356 38 : if (timed_requested) {
357 :
358 : //we are in this function BEFORE the begin of execution of a inner... so
359 : //we set the toWait to (afterMargin of previous inner + beforeMargin of actual (todo) inner)
360 : //except when timed_index == 0 where we have only the before
361 : //the after margin of last inner is not considered.
362 9 : if (timed_index == 0) {
363 4 : msToWait = timedAction->getAllActionMargins().at(timed_index).first;
364 :
365 5 : } else if (timed_index < timedAction->getInnerActionsNames().size()) {
366 15 : msToWait = timedAction->getAllActionMargins().at(timed_index-1).second +
367 10 : timedAction->getAllActionMargins().at(timed_index).first;
368 : }
369 9 : msToWait *= 1000;
370 9 : timer.reset();
371 : }
372 :
373 38 : return true;
374 : }
375 :
376 2230 : double ROSEE::UniversalRosEndEffectorExecutor::sendFeedbackGoal(std::string currentAction) {
377 : //norm between goal and initial (when the goal arrived) position, but only considering the jointInvolved
378 :
379 2230 : double actualNorm = 0;
380 :
381 15440 : for ( auto it : _motor_involved_mask ) {
382 :
383 13210 : if ( it.second != 0 ) {
384 7455 : int id = -1;
385 7455 : _ee->getInternalIdForJoint ( it.first, id );
386 :
387 7455 : if( id >= 0 ) {
388 : // NOTE assume single joint
389 7455 : actualNorm += pow ( _qref[id] - _joint_actual_pos.at(it.first).at(0) , 2 );
390 : }
391 :
392 : else {
393 0 : ROS_ERROR_STREAM ( "YOU SHOULD NOT BE HERE, previous error should stop execution");
394 : }
395 : }
396 : }
397 2230 : actualNorm = sqrt(actualNorm);
398 :
399 : double actualCompletationPercentage;
400 :
401 2230 : if (actualNorm < _ros_action_server->getWantedNormError()) {
402 137 : actualCompletationPercentage = 100;
403 : } else {
404 :
405 : //NewValue = (((OldValue - OldMin) * (NewMax - NewMin)) / (OldMax - OldMin)) + NewMin
406 2093 : actualCompletationPercentage =
407 2093 : (((actualNorm - normGoalFromInitialPos) * (100-0)) / (0 - normGoalFromInitialPos)) + 0;
408 : }
409 :
410 2230 : _ros_action_server->sendFeedback(actualCompletationPercentage, currentAction);
411 2230 : return actualCompletationPercentage;
412 : }
413 :
414 :
415 21341 : bool ROSEE::UniversalRosEndEffectorExecutor::publish_motor_reference() {
416 :
417 21341 : _mr_msg.header.stamp = ros::Time::now();
418 21341 : _mr_msg.header.seq = _seq_id++;
419 :
420 21341 : _qref_filtered = _filt_q.process ( _qref );
421 :
422 21341 : int id = -1;
423 138189 : for ( const auto& motor_name : _motors_names ) {
424 :
425 : //id to put take the qref from the right index in qref
426 116848 : _ee->getInternalIdForJoint ( motor_name, id );
427 116848 : _mr_msg.name[id] = motor_name;
428 116848 : _mr_msg.position[id] =_qref_filtered[id] ;
429 116848 : _mr_msg.effort[id] = _qref_optional[id];
430 : }
431 :
432 21341 : _motor_reference_pub.publish ( _mr_msg );
433 :
434 21341 : return true;
435 : }
436 :
437 21724 : void ROSEE::UniversalRosEndEffectorExecutor::timer_callback ( const ros::TimerEvent& timer_ev ) {
438 :
439 : //this is true only when a new goal has arrived... so new goal ovewrite the old one
440 21724 : if (_ros_action_server->hasNewGoal()) {
441 :
442 33 : updateGoal(); //store jp of goal and update qref
443 :
444 21691 : } else if (_ros_action_server->hasGoal()) {
445 :
446 2230 : if (! timed_requested ) {
447 :
448 1392 : if (sendFeedbackGoal() >= 100) {
449 :
450 29 : _ros_action_server->sendComplete();
451 : }
452 :
453 : } else { //a timed is running
454 :
455 838 : if ( sendFeedbackGoal(timedAction->getInnerActionsNames().at(timed_index)) >= 100 ) {
456 :
457 108 : if (timed_index == timedAction->getInnerActionsNames().size()-1) {
458 : // completed all the timed action (the last inner has finished)
459 :
460 4 : _ros_action_server->sendComplete();
461 4 : timed_requested = false;
462 :
463 104 : } else if (
464 208 : timer.elapsed_time<double, std::chrono::milliseconds>() >
465 208 : (timedAction->getAllActionMargins().at(timed_index).second*1000) ) {
466 :
467 : // if the time passed is greater than the .after margin of the last executed,
468 : // then pass to the other action. This is done so we continue to send feedback
469 : // of a inner (with value 100) until the margin after is passed. If we are here,
470 : // a inner action (but not the last one) is completed (included after margin),
471 : // so we have to execute the next one now
472 :
473 5 : timed_index++;
474 5 : _motor_position_goal = timedAction->getAllJointPos().at(timed_index);
475 5 : _motor_involved_mask = timedAction->getAllJointCountAction().at(timed_index);
476 5 : updateRefGoal();
477 :
478 : }
479 : }
480 : }
481 : }
482 :
483 :
484 : //if it is a timed, we have to check if we need to still wait to execute the subsequent innet action
485 21724 : if (timed_requested) {
486 838 : if (timer.elapsed_time<double, std::chrono::milliseconds>() > msToWait ) {
487 455 : publish_motor_reference();
488 :
489 : } else {
490 383 : ROS_INFO_STREAM ("Waiting time to execute timed action...");
491 : }
492 :
493 : } else {
494 20886 : publish_motor_reference();
495 : }
496 :
497 : // update time
498 21724 : _time += _period;
499 21724 : }
500 :
501 :
502 41 : void ROSEE::UniversalRosEndEffectorExecutor::spin() {
503 :
504 41 : _loop_timer.start();
505 :
506 41 : ROS_INFO_STREAM ( "Started looping @ " << 1./_period << "Hz" );
507 :
508 41 : ros::spin();
509 41 : }
510 :
511 :
512 :
513 41 : ROSEE::UniversalRosEndEffectorExecutor::~UniversalRosEndEffectorExecutor() {
514 :
515 164 : }
|
