ROSEndEffector
ROS End-Effector package: provides a ROS-based set of standard interfaces to command robotics end-effectors in an agnostic fashion.
Public Types | Public Member Functions | Private Member Functions | Private Attributes | List of all members
ROSEE::UniversalRosEndEffectorExecutor Class Reference

Class representing the ROS executor implementing the unviversal ROS EE concept. More...

#include <UniversalRosEndEffectorExecutor.h>

+ Collaboration diagram for ROSEE::UniversalRosEndEffectorExecutor:

Public Types

typedef std::shared_ptr< UniversalRosEndEffectorExecutorPtr
 
typedef std::shared_ptr< const UniversalRosEndEffectorExecutorConstPtr
 

Public Member Functions

 UniversalRosEndEffectorExecutor (std::string ns="")
 
virtual ~UniversalRosEndEffectorExecutor ()
 
void spin ()
 
void timer_callback (const ros::TimerEvent &timer_ev)
 

Private Member Functions

bool init_motor_reference_pub ()
 
bool init_qref_filter ()
 
void init_joint_state_sub ()
 
bool init_grasping_primitive ()
 
bool publish_motor_reference ()
 
bool updateGoal ()
 
bool readOptionalCommands (std::vector< std::string > motors_names, std::vector< double > motors_commands)
 
bool updateRefGoal (double percentage=1.0)
 
double sendFeedbackGoal (std::string currentAction="")
 
bool update_send_timed ()
 
void joint_state_clbk (const sensor_msgs::JointStateConstPtr &msg)
 

Private Attributes

ros::NodeHandle _nh
 
ros::Timer _loop_timer
 
double _time
 
double _period
 
double _rate
 
ROSEE::EEInterface::Ptr _ee
 
ros::Publisher _motor_reference_pub
 
sensor_msgs::JointState _mr_msg
 
int _motors_num = 0
 
int _seq_id = 0
 
ROSEE::JointPos _joint_actual_pos
 
ros::Subscriber _joint_state_sub
 
std::vector< std::string > _motors_names
 
std::string folderForActions
 
Eigen::VectorXd _qref
 
Eigen::VectorXd _qref_filtered
 
Eigen::VectorXd _qref_optional
 
ROSEE::Utils::SecondOrderFilter< Eigen::VectorXd > _filt_q
 
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr_pinchParsedMap
 
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr_pinchLooseParsedMap
 
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr_trigParsedMap
 
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr_tipFlexParsedMap
 
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr_fingFlexParsedMap
 
std::shared_ptr< ROSEE::ActionGeneric_graspParsed
 
MapActionHandler::Ptr mapActionHandlerPtr
 
std::shared_ptr< RosServiceHandler_ros_service_handler
 
std::shared_ptr< RosActionServer_ros_action_server
 
ROSEE::JointsInvolvedCount _motor_involved_mask
 
ROSEE::JointPos _motor_position_goal
 
double normGoalFromInitialPos
 
bool timed_requested
 
std::shared_ptr< ROSEE::ActionTimedtimedAction
 
unsigned int timed_index
 
ROSEE::Utils::Timer timer
 
double msToWait
 

Detailed Description

Class representing the ROS executor implementing the unviversal ROS EE concept.

Definition at line 48 of file UniversalRosEndEffectorExecutor.h.

Member Typedef Documentation

Definition at line 54 of file UniversalRosEndEffectorExecutor.h.

Definition at line 53 of file UniversalRosEndEffectorExecutor.h.

Constructor & Destructor Documentation

ROSEE::UniversalRosEndEffectorExecutor::UniversalRosEndEffectorExecutor ( std::string  ns = "")

Definition at line 21 of file UniversalRosEndEffectorExecutor.cpp.

21  : _nh ( ns ) {
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
35  p.printEndEffectorFingerJointsMap();
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 
44  folderForActions = p.getActionPath();
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 }
void timer_callback(const ros::TimerEvent &timer_ev)
std::shared_ptr< RosServiceHandler > _ros_service_handler
std::shared_ptr< RosActionServer > _ros_action_server
Class responsible for parsing the YAML file providing the URDF, SRDF, the EE-HAL library implementati...
Definition: Parser.h:46
ROSEE::UniversalRosEndEffectorExecutor::~UniversalRosEndEffectorExecutor ( )
virtual

Definition at line 510 of file UniversalRosEndEffectorExecutor.cpp.

510  {
511 
512 }

Member Function Documentation

bool ROSEE::UniversalRosEndEffectorExecutor::init_grasping_primitive ( )
private

Definition at line 115 of file UniversalRosEndEffectorExecutor.cpp.

115  {
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 }
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr > _pinchLooseParsedMap
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr > _fingFlexParsedMap
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr > _pinchParsedMap
std::shared_ptr< ROSEE::ActionGeneric > _graspParsed
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr > _tipFlexParsedMap
std::map< std::set< std::string >, ROSEE::ActionPrimitive::Ptr > _trigParsedMap
void ROSEE::UniversalRosEndEffectorExecutor::init_joint_state_sub ( )
private

Definition at line 169 of file UniversalRosEndEffectorExecutor.cpp.

169  {
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 }
void joint_state_clbk(const sensor_msgs::JointStateConstPtr &msg)
bool ROSEE::UniversalRosEndEffectorExecutor::init_motor_reference_pub ( )
private

Definition at line 72 of file UniversalRosEndEffectorExecutor.cpp.

72  {
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 }
bool ROSEE::UniversalRosEndEffectorExecutor::init_qref_filter ( )
private

Definition at line 91 of file UniversalRosEndEffectorExecutor.cpp.

91  {
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 }
void setOmega(double omega)
Definition: Utils.h:356
void setDamping(double eps)
Definition: Utils.h:367
ROSEE::Utils::SecondOrderFilter< Eigen::VectorXd > _filt_q
void setTimeStep(double ts)
Definition: Utils.h:378
void reset(const SignalType &initial_state)
Definition: Utils.h:320
void ROSEE::UniversalRosEndEffectorExecutor::joint_state_clbk ( const sensor_msgs::JointStateConstPtr &  msg)
private

Definition at line 180 of file UniversalRosEndEffectorExecutor.cpp.

180  {
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 }
bool ROSEE::UniversalRosEndEffectorExecutor::publish_motor_reference ( )
private

Definition at line 412 of file UniversalRosEndEffectorExecutor.cpp.

412  {
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 }
const SignalType & process(const SignalType &input)
Definition: Utils.h:331
ROSEE::Utils::SecondOrderFilter< Eigen::VectorXd > _filt_q
bool ROSEE::UniversalRosEndEffectorExecutor::readOptionalCommands ( std::vector< std::string >  motors_names,
std::vector< double >  motors_commands 
)
private

Definition at line 292 of file UniversalRosEndEffectorExecutor.cpp.

293  {
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 }
double ROSEE::UniversalRosEndEffectorExecutor::sendFeedbackGoal ( std::string  currentAction = "")
private

Definition at line 373 of file UniversalRosEndEffectorExecutor.cpp.

373  {
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 }
std::shared_ptr< RosActionServer > _ros_action_server
void ROSEE::UniversalRosEndEffectorExecutor::spin ( )

Definition at line 499 of file UniversalRosEndEffectorExecutor.cpp.

499  {
500 
501  _loop_timer.start();
502 
503  ROS_INFO_STREAM ( "Started looping @ " << 1./_period << "Hz" );
504 
505  ros::spin();
506 }
void ROSEE::UniversalRosEndEffectorExecutor::timer_callback ( const ros::TimerEvent &  timer_ev)

Definition at line 434 of file UniversalRosEndEffectorExecutor.cpp.

434  {
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 }
std::shared_ptr< RosActionServer > _ros_action_server
double sendFeedbackGoal(std::string currentAction="")
std::shared_ptr< ROSEE::ActionTimed > timedAction
Rep elapsed_time() const
Definition: Utils.h:284
bool ROSEE::UniversalRosEndEffectorExecutor::update_send_timed ( )
private
bool ROSEE::UniversalRosEndEffectorExecutor::updateGoal ( )
private

Definition at line 191 of file UniversalRosEndEffectorExecutor.cpp.

191  {
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 }
std::shared_ptr< RosActionServer > _ros_action_server
std::shared_ptr< ActionGeneric > Ptr
Definition: ActionGeneric.h:35
std::shared_ptr< ActionPrimitive > Ptr
std::shared_ptr< ROSEE::ActionTimed > timedAction
bool readOptionalCommands(std::vector< std::string > motors_names, std::vector< double > motors_commands)
bool ROSEE::UniversalRosEndEffectorExecutor::updateRefGoal ( double  percentage = 1.0)
private

Definition at line 328 of file UniversalRosEndEffectorExecutor.cpp.

328  {
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 }
std::shared_ptr< RosActionServer > _ros_action_server
std::shared_ptr< ROSEE::ActionTimed > timedAction

Member Data Documentation

ROSEE::EEInterface::Ptr ROSEE::UniversalRosEndEffectorExecutor::_ee
private

Definition at line 83 of file UniversalRosEndEffectorExecutor.h.

ROSEE::Utils::SecondOrderFilter<Eigen::VectorXd> ROSEE::UniversalRosEndEffectorExecutor::_filt_q
private

Definition at line 100 of file UniversalRosEndEffectorExecutor.h.

std::map<std::set<std::string>, ROSEE::ActionPrimitive::Ptr> ROSEE::UniversalRosEndEffectorExecutor::_fingFlexParsedMap
private

Definition at line 108 of file UniversalRosEndEffectorExecutor.h.

std::shared_ptr<ROSEE::ActionGeneric> ROSEE::UniversalRosEndEffectorExecutor::_graspParsed
private

Definition at line 110 of file UniversalRosEndEffectorExecutor.h.

ROSEE::JointPos ROSEE::UniversalRosEndEffectorExecutor::_joint_actual_pos
private

Definition at line 90 of file UniversalRosEndEffectorExecutor.h.

ros::Subscriber ROSEE::UniversalRosEndEffectorExecutor::_joint_state_sub
private

Definition at line 91 of file UniversalRosEndEffectorExecutor.h.

ros::Timer ROSEE::UniversalRosEndEffectorExecutor::_loop_timer
private

Definition at line 79 of file UniversalRosEndEffectorExecutor.h.

ROSEE::JointsInvolvedCount ROSEE::UniversalRosEndEffectorExecutor::_motor_involved_mask
private

Definition at line 117 of file UniversalRosEndEffectorExecutor.h.

ROSEE::JointPos ROSEE::UniversalRosEndEffectorExecutor::_motor_position_goal
private

Definition at line 118 of file UniversalRosEndEffectorExecutor.h.

ros::Publisher ROSEE::UniversalRosEndEffectorExecutor::_motor_reference_pub
private

Definition at line 85 of file UniversalRosEndEffectorExecutor.h.

std::vector<std::string> ROSEE::UniversalRosEndEffectorExecutor::_motors_names
private

Definition at line 94 of file UniversalRosEndEffectorExecutor.h.

int ROSEE::UniversalRosEndEffectorExecutor::_motors_num = 0
private

Definition at line 87 of file UniversalRosEndEffectorExecutor.h.

sensor_msgs::JointState ROSEE::UniversalRosEndEffectorExecutor::_mr_msg
private

Definition at line 86 of file UniversalRosEndEffectorExecutor.h.

ros::NodeHandle ROSEE::UniversalRosEndEffectorExecutor::_nh
private

Definition at line 78 of file UniversalRosEndEffectorExecutor.h.

double ROSEE::UniversalRosEndEffectorExecutor::_period
private

Definition at line 81 of file UniversalRosEndEffectorExecutor.h.

std::map<std::set<std::string>, ROSEE::ActionPrimitive::Ptr> ROSEE::UniversalRosEndEffectorExecutor::_pinchLooseParsedMap
private

Definition at line 105 of file UniversalRosEndEffectorExecutor.h.

std::map<std::set<std::string>, ROSEE::ActionPrimitive::Ptr> ROSEE::UniversalRosEndEffectorExecutor::_pinchParsedMap
private

Definition at line 104 of file UniversalRosEndEffectorExecutor.h.

Eigen::VectorXd ROSEE::UniversalRosEndEffectorExecutor::_qref
private

Definition at line 98 of file UniversalRosEndEffectorExecutor.h.

Eigen::VectorXd ROSEE::UniversalRosEndEffectorExecutor::_qref_filtered
private

Definition at line 98 of file UniversalRosEndEffectorExecutor.h.

Eigen::VectorXd ROSEE::UniversalRosEndEffectorExecutor::_qref_optional
private

Definition at line 98 of file UniversalRosEndEffectorExecutor.h.

double ROSEE::UniversalRosEndEffectorExecutor::_rate
private

Definition at line 81 of file UniversalRosEndEffectorExecutor.h.

std::shared_ptr<RosActionServer> ROSEE::UniversalRosEndEffectorExecutor::_ros_action_server
private

Definition at line 115 of file UniversalRosEndEffectorExecutor.h.

std::shared_ptr<RosServiceHandler> ROSEE::UniversalRosEndEffectorExecutor::_ros_service_handler
private

Definition at line 114 of file UniversalRosEndEffectorExecutor.h.

int ROSEE::UniversalRosEndEffectorExecutor::_seq_id = 0
private

Definition at line 88 of file UniversalRosEndEffectorExecutor.h.

double ROSEE::UniversalRosEndEffectorExecutor::_time
private

Definition at line 81 of file UniversalRosEndEffectorExecutor.h.

std::map<std::set<std::string>, ROSEE::ActionPrimitive::Ptr> ROSEE::UniversalRosEndEffectorExecutor::_tipFlexParsedMap
private

Definition at line 107 of file UniversalRosEndEffectorExecutor.h.

std::map<std::set<std::string>, ROSEE::ActionPrimitive::Ptr> ROSEE::UniversalRosEndEffectorExecutor::_trigParsedMap
private

Definition at line 106 of file UniversalRosEndEffectorExecutor.h.

std::string ROSEE::UniversalRosEndEffectorExecutor::folderForActions
private

Definition at line 96 of file UniversalRosEndEffectorExecutor.h.

MapActionHandler::Ptr ROSEE::UniversalRosEndEffectorExecutor::mapActionHandlerPtr
private

Definition at line 112 of file UniversalRosEndEffectorExecutor.h.

double ROSEE::UniversalRosEndEffectorExecutor::msToWait
private

Definition at line 125 of file UniversalRosEndEffectorExecutor.h.

double ROSEE::UniversalRosEndEffectorExecutor::normGoalFromInitialPos
private

Definition at line 119 of file UniversalRosEndEffectorExecutor.h.

unsigned int ROSEE::UniversalRosEndEffectorExecutor::timed_index
private

Definition at line 123 of file UniversalRosEndEffectorExecutor.h.

bool ROSEE::UniversalRosEndEffectorExecutor::timed_requested
private

Definition at line 121 of file UniversalRosEndEffectorExecutor.h.

std::shared_ptr<ROSEE::ActionTimed> ROSEE::UniversalRosEndEffectorExecutor::timedAction
private

Definition at line 122 of file UniversalRosEndEffectorExecutor.h.

ROSEE::Utils::Timer ROSEE::UniversalRosEndEffectorExecutor::timer
private

Definition at line 124 of file UniversalRosEndEffectorExecutor.h.


The documentation for this class was generated from the following files: