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

#include <RosServiceHandler.h>

+ Collaboration diagram for ROSEE::RosServiceHandler:

Public Member Functions

 RosServiceHandler (ros::NodeHandle *nh, ROSEE::MapActionHandler::Ptr, std::string path2saveYamlGeneric)
 Default constructor. More...
 
bool init (unsigned int nFinger)
 

Public Attributes

rosee_msg::HandInfo::Response handInfoResponse
 

Private Member Functions

bool selectablePairInfoCallback (rosee_msg::SelectablePairInfo::Request &request, rosee_msg::SelectablePairInfo::Response &response)
 
bool graspingActionsCallback (rosee_msg::GraspingActionsAvailable::Request &request, rosee_msg::GraspingActionsAvailable::Response &response)
 
rosee_msg::GraspingAction fillGraspingActionMsg (ROSEE::ActionPrimitive::Ptr primitive)
 
rosee_msg::GraspingAction fillGraspingActionMsg (ROSEE::ActionGeneric::Ptr generic)
 
rosee_msg::GraspingAction fillGraspingActionMsg (ROSEE::ActionTimed::Ptr timed)
 
void fillCommonInfoGraspingActionMsg (ROSEE::Action::Ptr action, rosee_msg::GraspingAction *msg)
 Internal function called by each of the fillGraspingActionMsg, it fills the GraspingAction message with the info that are always present in any kind of action (like name and type). More...
 
bool primitiveAggregatedCallback (rosee_msg::GraspingPrimitiveAggregatedAvailable::Request &request, rosee_msg::GraspingPrimitiveAggregatedAvailable::Response &response)
 
rosee_msg::GraspingPrimitiveAggregated fillPrimitiveAggregatedMsg (ROSEE::MapActionHandler::ActionPrimitiveMap primitiveMap)
 
rosee_msg::GraspingPrimitiveAggregated fillPrimitiveAggregatedMsg (ROSEE::ActionPrimitive::Ptr primitive)
 
bool handInfoCallback (rosee_msg::HandInfo::Request &request, rosee_msg::HandInfo::Response &response)
 
bool newGraspingActionCallback (rosee_msg::NewGenericGraspingActionSrv::Request &request, rosee_msg::NewGenericGraspingActionSrv::Response &response)
 

Private Attributes

MapActionHandler::Ptr _mapActionHandler
 
std::string _path2saveYamlGeneric
 
ros::NodeHandle * _nh
 
ros::ServiceServer _serverPrimitiveAggregated
 
ros::ServiceServer _server_selectablePairInfo
 
ros::ServiceServer _serverGraspingActions
 
ros::ServiceServer _serverNewGraspingAction
 
ros::ServiceServer _serverHandInfo
 
unsigned int nFinger
 

Detailed Description

Todo:
write docs

Definition at line 37 of file RosServiceHandler.h.

Constructor & Destructor Documentation

ROSEE::RosServiceHandler::RosServiceHandler ( ros::NodeHandle *  nh,
ROSEE::MapActionHandler::Ptr  mapActionHandler,
std::string  path2saveYamlGeneric 
)

Default constructor.

Definition at line 19 of file RosServiceHandler.cpp.

19  {
20 
21  if (mapActionHandler == nullptr) {
22  ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] the mapActionHandler in not initialized");
23  return;
24  }
25 
26  this->_mapActionHandler = mapActionHandler;
27  this->_path2saveYamlGeneric = path2saveYamlGeneric;
28 
29  this->_nh = nh;
30 
31 }
MapActionHandler::Ptr _mapActionHandler

Member Function Documentation

void ROSEE::RosServiceHandler::fillCommonInfoGraspingActionMsg ( ROSEE::Action::Ptr  action,
rosee_msg::GraspingAction *  msg 
)
private

Internal function called by each of the fillGraspingActionMsg, it fills the GraspingAction message with the info that are always present in any kind of action (like name and type).

Each specific fillGraspingActionMsg will insert fill the field specific for the action type (like time margins for timed actions)

Parameters
actionthe action that must be sent as response
msgthe ROS message that must be filled with action info

Definition at line 221 of file RosServiceHandler.cpp.

222  {
223 
224  graspingMsg->action_type = action->getType();
225  graspingMsg->action_name = action->getName();
226 
227  rosee_msg::JointsInvolvedCount motorCountMsg;
228  for ( auto motorCount : action->getJointsInvolvedCount()) {
229  motorCountMsg.name.push_back(motorCount.first);
230  motorCountMsg.count.push_back(motorCount.second);
231  }
232  graspingMsg->action_motor_count = motorCountMsg;
233 
234  for ( auto motorPosMultiple : action->getAllJointPos()) {
235 
236  //iterate over the single motor positions
237  rosee_msg::MotorPosition motorPosMsg;
238  for ( auto motorPos : motorPosMultiple) {
239  motorPosMsg.name.push_back(motorPos.first);
240  motorPosMsg.position.push_back(motorPos.second.at(0)); //at(0). because multiple dof is considered in general
241 
242  }
243  graspingMsg->action_motor_positions.push_back(motorPosMsg);
244  }
245 
246  for (auto elementInvolved : action->getFingersInvolved()) {
247  graspingMsg->elements_involved.push_back(elementInvolved);
248  }
249 }
std::map< std::string, unsigned int > JointsInvolvedCount
The map to describe, how many times a joint is set by the action.
Definition: Action.h:63
rosee_msg::GraspingAction ROSEE::RosServiceHandler::fillGraspingActionMsg ( ROSEE::ActionPrimitive::Ptr  primitive)
private

Definition at line 163 of file RosServiceHandler.cpp.

163  {
164 
165  rosee_msg::GraspingAction primitiveMsg;
166 
167  if (primitive == nullptr) {
168  return primitiveMsg;
169  }
170 
171  fillCommonInfoGraspingActionMsg(primitive, &primitiveMsg);
172 
173  primitiveMsg.primitive_type = primitive->getPrimitiveType();
174 
175  auto elements = primitive->getKeyElements();
176  primitiveMsg.elements_involved.assign(elements.begin(), elements.end());
177 
178  return primitiveMsg;
179 }
void fillCommonInfoGraspingActionMsg(ROSEE::Action::Ptr action, rosee_msg::GraspingAction *msg)
Internal function called by each of the fillGraspingActionMsg, it fills the GraspingAction message wi...
rosee_msg::GraspingAction ROSEE::RosServiceHandler::fillGraspingActionMsg ( ROSEE::ActionGeneric::Ptr  generic)
private

Definition at line 181 of file RosServiceHandler.cpp.

181  {
182 
183  rosee_msg::GraspingAction genericActionMsg;
184  if (generic == nullptr) {
185  return genericActionMsg;
186  }
187 
188  fillCommonInfoGraspingActionMsg(generic, &genericActionMsg);
189 
190  genericActionMsg.primitive_type = genericActionMsg.PRIMITIVE_NONE;
191 
192  ActionComposed::Ptr composedCasted = std::dynamic_pointer_cast<ActionComposed>(generic);
193  if ( composedCasted != nullptr) {
194  genericActionMsg.inner_actions = composedCasted->getInnerActionsNames();
195  }
196 
197  return genericActionMsg;
198 }
std::shared_ptr< ActionComposed > Ptr
void fillCommonInfoGraspingActionMsg(ROSEE::Action::Ptr action, rosee_msg::GraspingAction *msg)
Internal function called by each of the fillGraspingActionMsg, it fills the GraspingAction message wi...
rosee_msg::GraspingAction ROSEE::RosServiceHandler::fillGraspingActionMsg ( ROSEE::ActionTimed::Ptr  timed)
private

Definition at line 200 of file RosServiceHandler.cpp.

200  {
201 
202  rosee_msg::GraspingAction timedActionMsg;
203  if (timed == nullptr) {
204  return timedActionMsg;
205  }
206 
207  fillCommonInfoGraspingActionMsg(timed, &timedActionMsg);
208 
209  timedActionMsg.primitive_type = timedActionMsg.PRIMITIVE_NONE;
210 
211  timedActionMsg.inner_actions = timed->getInnerActionsNames();
212 
213  for (auto innerMargin : timed->getAllActionMargins()){
214  timedActionMsg.before_time_margins.push_back(innerMargin.first);
215  timedActionMsg.after_time_margins.push_back(innerMargin.second);
216  }
217 
218  return timedActionMsg;
219 }
void fillCommonInfoGraspingActionMsg(ROSEE::Action::Ptr action, rosee_msg::GraspingAction *msg)
Internal function called by each of the fillGraspingActionMsg, it fills the GraspingAction message wi...
rosee_msg::GraspingPrimitiveAggregated ROSEE::RosServiceHandler::fillPrimitiveAggregatedMsg ( ROSEE::MapActionHandler::ActionPrimitiveMap  primitiveMap)
private

Definition at line 304 of file RosServiceHandler.cpp.

305  {
306 
307  rosee_msg::GraspingPrimitiveAggregated primitiveMsg;
308 
309  primitiveMsg.action_name = primitiveMap.begin()->second->getName();
310  primitiveMsg.primitive_type = primitiveMap.begin()->second->getPrimitiveType();
311  //until now, there is not a primitive that does not have "something" to select
312  // (eg pinch has 2 fing, trig one fing, singleJointMultipleTips 1 joint...).
313  //Instead generic action has always no thing to select (next for loop)
314  primitiveMsg.max_selectable = primitiveMap.begin()->first.size();
315  //TODO extract the keys with another mapActionHandler function?
316  primitiveMsg.selectable_names =
318 
319  return primitiveMsg;
320 }
static std::vector< std::string > extract_keys_merged(std::map< std::set< std::string >, T > const &input_map, unsigned int max_string_number=0)
Extract all the string in the set keys of a map.
Definition: Utils.h:113
rosee_msg::GraspingPrimitiveAggregated ROSEE::RosServiceHandler::fillPrimitiveAggregatedMsg ( ROSEE::ActionPrimitive::Ptr  primitive)
private

Definition at line 322 of file RosServiceHandler.cpp.

323  {
324 
325  rosee_msg::GraspingPrimitiveAggregated primitiveMsg;
326  primitiveMsg.action_name = primitive->getName();
327  primitiveMsg.primitive_type = primitive->getPrimitiveType();
328 
329  auto elements = primitive->getKeyElements();
330  primitiveMsg.max_selectable = elements.size();
331  primitiveMsg.selectable_names.assign(elements.begin(), elements.end());
332 
333  return primitiveMsg;
334 }
bool ROSEE::RosServiceHandler::graspingActionsCallback ( rosee_msg::GraspingActionsAvailable::Request &  request,
rosee_msg::GraspingActionsAvailable::Response &  response 
)
private

Definition at line 65 of file RosServiceHandler.cpp.

67  {
68 
69  switch (request.action_type) {
70 
71  case 0 : { //PRIMITIVE
72 
73  //NOTE if both primitive type and action name are set in the request, the action name is not considered
74 
75  if (request.primitive_type == 0) {
76 
77  if (request.action_name.size() == 0 ) {
78  for (auto primitiveContainers : _mapActionHandler->getAllPrimitiveMaps() ) {
79 
80  //iterate all the primitive of a type
81  for (auto primitive : primitiveContainers.second) {
82  response.grasping_actions.push_back(fillGraspingActionMsg(primitive.second));
83  }
84  }
85 
86  } else {
87  if (request.elements_involved.size() == 0) {
88 
89  for (auto primitive : _mapActionHandler->getPrimitiveMap(request.action_name)) {
90  response.grasping_actions.push_back(fillGraspingActionMsg(primitive.second));
91  }
92 
93  } else {
94 
95  auto primitive =_mapActionHandler->getPrimitive(request.action_name, request.elements_involved);
96  response.grasping_actions.push_back(fillGraspingActionMsg(primitive));
97  }
98  }
99 
100  } else {
101 
102  if (request.elements_involved.size() == 0) {
103 
104  //NOTE -1 because in the srv 0 is for all primitives, then the enum is scaled by one
105  for (auto primitives : _mapActionHandler->getPrimitiveMap(
106  static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1))) {
107 
108  for (auto primitive : primitives) {
109  response.grasping_actions.push_back(fillGraspingActionMsg(primitive.second));
110  }
111  }
112 
113  } else {
114 
115  for (auto primitive : _mapActionHandler->getPrimitive(
116  static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1), request.elements_involved)) {
117 
118  response.grasping_actions.push_back(fillGraspingActionMsg(primitive));
119  }
120  }
121  }
122 
123  break;
124  }
125 
126  case 1 : { //GENERIC_and_COMPOSED
127 
128  //NOTE for these some fields are ignored
129  if (request.action_name.size() == 0) {
130  for (auto action : _mapActionHandler->getAllGenerics()) {
131  response.grasping_actions.push_back(fillGraspingActionMsg(action.second));
132  }
133 
134  } else {
135  response.grasping_actions.push_back(fillGraspingActionMsg(_mapActionHandler->getGeneric(request.action_name)));
136  }
137  break;
138  }
139 
140  case 2 : { //TIMED
141  if (request.action_name.size() == 0) {
142  for (auto action : _mapActionHandler->getAllTimeds()) {
143  response.grasping_actions.push_back(fillGraspingActionMsg(action.second));
144  }
145 
146  } else {
147  response.grasping_actions.push_back(fillGraspingActionMsg(_mapActionHandler->getTimed(request.action_name)));
148  }
149  break;
150  }
151 
152  default : {
153  ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] request.actionType can only be 0(ALL), 1(PRIMITIVE), "
154  << "2(GENERIC_and_COMPOSED), or 3(TIMED); I have received " << request.action_type);
155  return false;
156 
157  }
158  }
159 
160  return true;
161 }
MapActionHandler::Ptr _mapActionHandler
rosee_msg::GraspingAction fillGraspingActionMsg(ROSEE::ActionPrimitive::Ptr primitive)
bool ROSEE::RosServiceHandler::handInfoCallback ( rosee_msg::HandInfo::Request &  request,
rosee_msg::HandInfo::Response &  response 
)
private

Definition at line 370 of file RosServiceHandler.cpp.

372  {
373 
374  if (! ros::service::exists("/EEHalExecutor/hand_info", false) ) {
375  return false;
376  }
377 
378  ros::ServiceClient handInfoClient =
379  _nh->serviceClient<rosee_msg::HandInfo>("/EEHalExecutor/hand_info");
380  rosee_msg::HandInfo handInfoMsg;
381 
382  //request may be empty or not according to which hal we are using
383  handInfoMsg.request = request;
384  if (handInfoClient.call(handInfoMsg)) {
385 
386  response = handInfoMsg.response;
387 
388  } else {
389  return false;
390  }
391 
392  return true;
393 }
bool ROSEE::RosServiceHandler::init ( unsigned int  nFinger)

Definition at line 34 of file RosServiceHandler.cpp.

34  {
35 
36  this->nFinger = nFinger;
37 
38  std::string graspingActionsSrvName, actionInfoServiceName,
39  selectablePairInfoServiceName, handInfoServiceName, newGraspingActionServiceName;
40 
41  _nh->param<std::string>("/rosee/grasping_action_srv_name", graspingActionsSrvName, "grasping_actions_available");
42  _nh->param<std::string>("/rosee/primitive_aggregated_srv_name", actionInfoServiceName, "primitives_aggregated_available");
43  _nh->param<std::string>("/rosee/selectable_finger_pair_info", selectablePairInfoServiceName, "selectable_finger_pair_info");
44  _nh->param<std::string>("/rosee/hand_info", handInfoServiceName, "hand_info");
45  _nh->param<std::string>("/rosee/new_grasping_action_srv_name", newGraspingActionServiceName, "new_generic_grasping_action");
46 
47  _serverGraspingActions = _nh->advertiseService(graspingActionsSrvName,
49 
50  _serverPrimitiveAggregated = _nh->advertiseService(actionInfoServiceName,
52 
53  _server_selectablePairInfo = _nh->advertiseService(selectablePairInfoServiceName,
55 
56  _serverHandInfo = _nh->advertiseService(handInfoServiceName,
58 
59  _serverNewGraspingAction = _nh->advertiseService(newGraspingActionServiceName,
61 
62  return true;
63 }
bool graspingActionsCallback(rosee_msg::GraspingActionsAvailable::Request &request, rosee_msg::GraspingActionsAvailable::Response &response)
bool handInfoCallback(rosee_msg::HandInfo::Request &request, rosee_msg::HandInfo::Response &response)
bool newGraspingActionCallback(rosee_msg::NewGenericGraspingActionSrv::Request &request, rosee_msg::NewGenericGraspingActionSrv::Response &response)
ros::ServiceServer _serverNewGraspingAction
ros::ServiceServer _serverPrimitiveAggregated
ros::ServiceServer _serverHandInfo
ros::ServiceServer _server_selectablePairInfo
ros::ServiceServer _serverGraspingActions
bool primitiveAggregatedCallback(rosee_msg::GraspingPrimitiveAggregatedAvailable::Request &request, rosee_msg::GraspingPrimitiveAggregatedAvailable::Response &response)
bool selectablePairInfoCallback(rosee_msg::SelectablePairInfo::Request &request, rosee_msg::SelectablePairInfo::Response &response)
bool ROSEE::RosServiceHandler::newGraspingActionCallback ( rosee_msg::NewGenericGraspingActionSrv::Request &  request,
rosee_msg::NewGenericGraspingActionSrv::Response &  response 
)
private

Definition at line 397 of file RosServiceHandler.cpp.

399  {
400 
401  response.accepted = false;
402  response.emitted = false;
403 
404  if (request.newAction.action_name.empty()) {
405 
406  response.error_msg = "action_name can not be empty";
407  ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
408  return true; //so the client receive the response
409  }
410 
411  if (request.newAction.action_motor_position.name.size() == 0 ||
412  request.newAction.action_motor_position.position.size() == 0 ||
413  request.newAction.action_motor_position.position.size() != request.newAction.action_motor_position.name.size()) {
414 
415  response.error_msg = "action_motor_position is empty or badly formed";
416  ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
417 
418  return true; //so the client receive the response
419  }
420 
421  if (request.newAction.action_motor_count.name.size() != request.newAction.action_motor_count.count.size()) {
422 
423  response.error_msg = "action_motor_count is badly formed, name and count have different sizes";
424  ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
425 
426  return true; //so the client receive the response
427  }
428 
429  // TODO request.newAction.action_motor_count : if empty, ActionGeneric costructor will consider all joint
430  // with 0 position as not used. This may change in future when we will support not 0 default joint positions
431 
432  if (_mapActionHandler->getGeneric(request.newAction.action_name, false) != nullptr) {
433 
434  response.error_msg = "A generic action with name '" + request.newAction.action_name + "' already exists";
435  ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
436 
437  return true; //so the client receive the response
438 
439  }
440 
441  ROSEE::ActionGeneric::Ptr newAction;
442  ROSEE::JointPos jp;
444  std::set<std::string> elementInvolved;
445 
446  for (int i = 0; i < request.newAction.action_motor_position.name.size(); i++){
447 
448  std::vector<double> one_dof{request.newAction.action_motor_position.position.at(i)};
449  jp.insert(std::make_pair(request.newAction.action_motor_position.name.at(i),
450  one_dof));
451  }
452 
453  for (int i = 0; i < request.newAction.action_motor_count.name.size(); i++){
454 
455  jic.insert(std::make_pair(request.newAction.action_motor_count.name.at(i),
456  request.newAction.action_motor_count.count.at(i)));
457  }
458 
459  for (int i = 0; i< request.newAction.elements_involved.size(); i++) {
460 
461  elementInvolved.insert(request.newAction.elements_involved.at(i));
462  }
463 
464  //costructor will handle jpc and elementInvolved also if empty
465  try { newAction = std::make_shared<ROSEE::ActionGeneric>(request.newAction.action_name,
466  jp,
467  jic,
468  elementInvolved);
469 
471 
472  response.error_msg = "action_motor_position and action_motor_count have different names element. They must be the same because they refer to actuator of the end-effector";
473  ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
474 
475  return true; //so the client receive the response
476  }
477 
478  //u rosee main node use always mapActionHandler to check if an action exists. Thus, we need to add this new
479  // action into the mapActionHandler "database" (ie private member map of the generic actions)
480  if (! _mapActionHandler->insertSingleGeneric(newAction)){
481 
482  response.error_msg = "error by mapActionHandler when inserting the new generic action";
483  ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
484 
485  return true; //so the client receive the response
486  }
487 
488  ROS_INFO_STREAM ( "[RosServiceHandler " << __func__ << " ] The new action '"<< newAction->getName() << "' is inserted in the system");
489 
490 
491  if (request.emitYaml) {
492 
493  ROSEE::YamlWorker yamlWorker;
494  auto path = yamlWorker.createYamlFile(newAction, _path2saveYamlGeneric);
495  ROS_INFO_STREAM ( "[RosServiceHandler " << __func__ << " ] The received new action '"<< newAction->getName() << "' has been stored in " << path);
496  response.emitted = true;
497  }
498 
499  response.accepted = true;
500  return true;
501 }
std::map< std::string, std::vector< double > > JointPos
The map to describe the position of all actuated joints.
Definition: Action.h:40
std::map< std::string, unsigned int > JointsInvolvedCount
The map to describe, how many times a joint is set by the action.
Definition: Action.h:63
MapActionHandler::Ptr _mapActionHandler
std::shared_ptr< ActionGeneric > Ptr
Definition: ActionGeneric.h:35
std::string createYamlFile(const std::map< std::set< std::string >, ActionPrimitive * > mapOfActions, const std::string actionName, std::string pathFolder)
Create/overwrite yaml file and emit info on it about each ActionPrimitive inside the given mapOfActio...
Definition: YamlWorker.cpp:39
bool ROSEE::RosServiceHandler::primitiveAggregatedCallback ( rosee_msg::GraspingPrimitiveAggregatedAvailable::Request &  request,
rosee_msg::GraspingPrimitiveAggregatedAvailable::Response &  response 
)
private

Definition at line 251 of file RosServiceHandler.cpp.

253  {
254 
255  if (request.primitive_type == 0) {
256 
257  if (request.action_name.size() == 0 ) {
258  // return all primitives
259 
260  for (auto primitiveMaps : _mapActionHandler->getAllPrimitiveMaps() ) {
261 
262  response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitiveMaps.second));
263  }
264 
265  } else {
266  if (request.elements_involved.size() == 0) {
267 
268  auto primitiveMap = _mapActionHandler->getPrimitiveMap(request.action_name);
269  response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitiveMap));
270 
271 
272  } else {
273 
274  auto primitive =_mapActionHandler->getPrimitive(request.action_name, request.elements_involved);
275  response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitive));
276  }
277  }
278 
279  } else {
280 
281  if (request.elements_involved.size() == 0) {
282 
283  //NOTE -1 because in the srv 0 is for all primitives, then the enum is scaled by one
284  for (auto primitiveMap : _mapActionHandler->getPrimitiveMap(
285  static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1))) {
286 
287  response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitiveMap));
288 
289  }
290 
291  } else {
292 
293  for (auto primitive : _mapActionHandler->getPrimitive(
294  static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1), request.elements_involved)) {
295 
296  response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitive));
297  }
298  }
299  }
300  return true;
301 }
MapActionHandler::Ptr _mapActionHandler
rosee_msg::GraspingPrimitiveAggregated fillPrimitiveAggregatedMsg(ROSEE::MapActionHandler::ActionPrimitiveMap primitiveMap)
bool ROSEE::RosServiceHandler::selectablePairInfoCallback ( rosee_msg::SelectablePairInfo::Request &  request,
rosee_msg::SelectablePairInfo::Response &  response 
)
private

Definition at line 337 of file RosServiceHandler.cpp.

339  {
340 
341  std::set<std::string> companionFingers;
342  if (request.action_name.compare ("pinchTight") == 0) {
343  companionFingers =
344  _mapActionHandler->getFingertipsForPinch(request.element_name,
345  ROSEE::ActionPrimitive::Type::PinchTight) ;
346 
347  } else if (request.action_name.compare ("pinchLoose") == 0) {
348  companionFingers =
349  _mapActionHandler->getFingertipsForPinch(request.element_name,
350  ROSEE::ActionPrimitive::Type::PinchLoose) ;
351 
352  } else {
353  ROS_ERROR_STREAM ( "Received" << request.action_name << " that is not" <<
354  "a recognizible action name to look for finger companions" );
355  return false;
356  }
357 
358  if (companionFingers.size() == 0) {
359  return false;
360  }
361 
362  //push the elements of set into the vector
363  for (auto fing : companionFingers ) {
364  response.pair_elements.push_back (fing);
365  }
366 
367  return true;
368 }
MapActionHandler::Ptr _mapActionHandler

Member Data Documentation

MapActionHandler::Ptr ROSEE::RosServiceHandler::_mapActionHandler
private

Definition at line 50 of file RosServiceHandler.h.

ros::NodeHandle* ROSEE::RosServiceHandler::_nh
private

Definition at line 52 of file RosServiceHandler.h.

std::string ROSEE::RosServiceHandler::_path2saveYamlGeneric
private

Definition at line 51 of file RosServiceHandler.h.

ros::ServiceServer ROSEE::RosServiceHandler::_server_selectablePairInfo
private

Definition at line 54 of file RosServiceHandler.h.

ros::ServiceServer ROSEE::RosServiceHandler::_serverGraspingActions
private

Definition at line 55 of file RosServiceHandler.h.

ros::ServiceServer ROSEE::RosServiceHandler::_serverHandInfo
private

Definition at line 58 of file RosServiceHandler.h.

ros::ServiceServer ROSEE::RosServiceHandler::_serverNewGraspingAction
private

Definition at line 56 of file RosServiceHandler.h.

ros::ServiceServer ROSEE::RosServiceHandler::_serverPrimitiveAggregated
private

Definition at line 53 of file RosServiceHandler.h.

rosee_msg::HandInfo::Response ROSEE::RosServiceHandler::handInfoResponse

Definition at line 47 of file RosServiceHandler.h.

unsigned int ROSEE::RosServiceHandler::nFinger
private

Definition at line 61 of file RosServiceHandler.h.


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