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