Line data Source code
1 : /*
2 : * Copyright 2020 <copyright holder> <email>
3 : *
4 : * Licensed under the Apache License, Version 2.0 (the "License");
5 : * you may not use this file except in compliance with the License.
6 : * You may obtain a copy of the License at
7 : *
8 : * http://www.apache.org/licenses/LICENSE-2.0
9 : *
10 : * Unless required by applicable law or agreed to in writing, software
11 : * distributed under the License is distributed on an "AS IS" BASIS,
12 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 : * See the License for the specific language governing permissions and
14 : * limitations under the License.
15 : */
16 :
17 : #include <ros_end_effector/RosServiceHandler.h>
18 :
19 41 : ROSEE::RosServiceHandler::RosServiceHandler( ros::NodeHandle *nh, ROSEE::MapActionHandler::Ptr mapActionHandler, std::string path2saveYamlGeneric) {
20 :
21 41 : if (mapActionHandler == nullptr) {
22 0 : ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] the mapActionHandler in not initialized");
23 0 : return;
24 : }
25 :
26 41 : this->_mapActionHandler = mapActionHandler;
27 41 : this->_path2saveYamlGeneric = path2saveYamlGeneric;
28 :
29 41 : this->_nh = nh;
30 :
31 : }
32 :
33 : //TODO see if this argument can be avoided, maybe use extract_keys_merged in map action handler?
34 41 : bool ROSEE::RosServiceHandler::init(unsigned int nFinger) {
35 :
36 41 : this->nFinger = nFinger;
37 :
38 82 : std::string graspingActionsSrvName, actionInfoServiceName,
39 82 : selectablePairInfoServiceName, handInfoServiceName, newGraspingActionServiceName;
40 :
41 41 : _nh->param<std::string>("/rosee/grasping_action_srv_name", graspingActionsSrvName, "grasping_actions_available");
42 41 : _nh->param<std::string>("/rosee/primitive_aggregated_srv_name", actionInfoServiceName, "primitives_aggregated_available");
43 41 : _nh->param<std::string>("/rosee/selectable_finger_pair_info", selectablePairInfoServiceName, "selectable_finger_pair_info");
44 41 : _nh->param<std::string>("/rosee/hand_info", handInfoServiceName, "hand_info");
45 41 : _nh->param<std::string>("/rosee/new_grasping_action_srv_name", newGraspingActionServiceName, "new_generic_grasping_action");
46 :
47 82 : _serverGraspingActions = _nh->advertiseService(graspingActionsSrvName,
48 41 : &RosServiceHandler::graspingActionsCallback, this);
49 :
50 82 : _serverPrimitiveAggregated = _nh->advertiseService(actionInfoServiceName,
51 41 : &RosServiceHandler::primitiveAggregatedCallback, this);
52 :
53 82 : _server_selectablePairInfo = _nh->advertiseService(selectablePairInfoServiceName,
54 41 : &RosServiceHandler::selectablePairInfoCallback, this);
55 :
56 82 : _serverHandInfo = _nh->advertiseService(handInfoServiceName,
57 41 : &RosServiceHandler::handInfoCallback, this);
58 :
59 82 : _serverNewGraspingAction = _nh->advertiseService(newGraspingActionServiceName,
60 41 : &RosServiceHandler::newGraspingActionCallback, this);
61 :
62 82 : return true;
63 : }
64 :
65 8 : bool ROSEE::RosServiceHandler::graspingActionsCallback(
66 : rosee_msg::GraspingActionsAvailable::Request& request,
67 : rosee_msg::GraspingActionsAvailable::Response& response) {
68 :
69 8 : switch (request.action_type) {
70 :
71 4 : 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 4 : if (request.primitive_type == 0) {
76 :
77 4 : if (request.action_name.size() == 0 ) {
78 0 : for (auto primitiveContainers : _mapActionHandler->getAllPrimitiveMaps() ) {
79 :
80 : //iterate all the primitive of a type
81 0 : for (auto primitive : primitiveContainers.second) {
82 0 : response.grasping_actions.push_back(fillGraspingActionMsg(primitive.second));
83 : }
84 : }
85 :
86 : } else {
87 4 : if (request.elements_involved.size() == 0) {
88 :
89 4 : for (auto primitive : _mapActionHandler->getPrimitiveMap(request.action_name)) {
90 0 : response.grasping_actions.push_back(fillGraspingActionMsg(primitive.second));
91 : }
92 :
93 : } else {
94 :
95 0 : auto primitive =_mapActionHandler->getPrimitive(request.action_name, request.elements_involved);
96 0 : response.grasping_actions.push_back(fillGraspingActionMsg(primitive));
97 : }
98 : }
99 :
100 : } else {
101 :
102 0 : 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 0 : for (auto primitives : _mapActionHandler->getPrimitiveMap(
106 0 : static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1))) {
107 :
108 0 : for (auto primitive : primitives) {
109 0 : response.grasping_actions.push_back(fillGraspingActionMsg(primitive.second));
110 : }
111 : }
112 :
113 : } else {
114 :
115 0 : for (auto primitive : _mapActionHandler->getPrimitive(
116 0 : static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1), request.elements_involved)) {
117 :
118 0 : response.grasping_actions.push_back(fillGraspingActionMsg(primitive));
119 : }
120 : }
121 : }
122 :
123 4 : break;
124 : }
125 :
126 4 : case 1 : { //GENERIC_and_COMPOSED
127 :
128 : //NOTE for these some fields are ignored
129 4 : if (request.action_name.size() == 0) {
130 0 : for (auto action : _mapActionHandler->getAllGenerics()) {
131 0 : response.grasping_actions.push_back(fillGraspingActionMsg(action.second));
132 : }
133 :
134 : } else {
135 4 : response.grasping_actions.push_back(fillGraspingActionMsg(_mapActionHandler->getGeneric(request.action_name)));
136 : }
137 4 : break;
138 : }
139 :
140 0 : case 2 : { //TIMED
141 0 : if (request.action_name.size() == 0) {
142 0 : for (auto action : _mapActionHandler->getAllTimeds()) {
143 0 : response.grasping_actions.push_back(fillGraspingActionMsg(action.second));
144 : }
145 :
146 : } else {
147 0 : response.grasping_actions.push_back(fillGraspingActionMsg(_mapActionHandler->getTimed(request.action_name)));
148 : }
149 0 : break;
150 : }
151 :
152 0 : default : {
153 0 : 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 0 : return false;
156 :
157 : }
158 : }
159 :
160 8 : return true;
161 : }
162 :
163 0 : rosee_msg::GraspingAction ROSEE::RosServiceHandler::fillGraspingActionMsg(ROSEE::ActionPrimitive::Ptr primitive) {
164 :
165 0 : rosee_msg::GraspingAction primitiveMsg;
166 :
167 0 : if (primitive == nullptr) {
168 0 : return primitiveMsg;
169 : }
170 :
171 0 : fillCommonInfoGraspingActionMsg(primitive, &primitiveMsg);
172 :
173 0 : primitiveMsg.primitive_type = primitive->getPrimitiveType();
174 :
175 0 : auto elements = primitive->getKeyElements();
176 0 : primitiveMsg.elements_involved.assign(elements.begin(), elements.end());
177 :
178 0 : return primitiveMsg;
179 : }
180 :
181 4 : rosee_msg::GraspingAction ROSEE::RosServiceHandler::fillGraspingActionMsg(ROSEE::ActionGeneric::Ptr generic) {
182 :
183 4 : rosee_msg::GraspingAction genericActionMsg;
184 4 : if (generic == nullptr) {
185 0 : return genericActionMsg;
186 : }
187 :
188 4 : fillCommonInfoGraspingActionMsg(generic, &genericActionMsg);
189 :
190 4 : genericActionMsg.primitive_type = genericActionMsg.PRIMITIVE_NONE;
191 :
192 8 : ActionComposed::Ptr composedCasted = std::dynamic_pointer_cast<ActionComposed>(generic);
193 4 : if ( composedCasted != nullptr) {
194 0 : genericActionMsg.inner_actions = composedCasted->getInnerActionsNames();
195 : }
196 :
197 4 : return genericActionMsg;
198 : }
199 :
200 0 : rosee_msg::GraspingAction ROSEE::RosServiceHandler::fillGraspingActionMsg(ROSEE::ActionTimed::Ptr timed) {
201 :
202 0 : rosee_msg::GraspingAction timedActionMsg;
203 0 : if (timed == nullptr) {
204 0 : return timedActionMsg;
205 : }
206 :
207 0 : fillCommonInfoGraspingActionMsg(timed, &timedActionMsg);
208 :
209 0 : timedActionMsg.primitive_type = timedActionMsg.PRIMITIVE_NONE;
210 :
211 0 : timedActionMsg.inner_actions = timed->getInnerActionsNames();
212 :
213 0 : for (auto innerMargin : timed->getAllActionMargins()){
214 0 : timedActionMsg.before_time_margins.push_back(innerMargin.first);
215 0 : timedActionMsg.after_time_margins.push_back(innerMargin.second);
216 : }
217 :
218 0 : return timedActionMsg;
219 : }
220 :
221 4 : void ROSEE::RosServiceHandler::fillCommonInfoGraspingActionMsg(ROSEE::Action::Ptr action,
222 : rosee_msg::GraspingAction* graspingMsg) {
223 :
224 4 : graspingMsg->action_type = action->getType();
225 4 : graspingMsg->action_name = action->getName();
226 :
227 8 : rosee_msg::JointsInvolvedCount motorCountMsg;
228 8 : for ( auto motorCount : action->getJointsInvolvedCount()) {
229 4 : motorCountMsg.name.push_back(motorCount.first);
230 4 : motorCountMsg.count.push_back(motorCount.second);
231 : }
232 4 : graspingMsg->action_motor_count = motorCountMsg;
233 :
234 8 : for ( auto motorPosMultiple : action->getAllJointPos()) {
235 :
236 : //iterate over the single motor positions
237 8 : rosee_msg::MotorPosition motorPosMsg;
238 8 : for ( auto motorPos : motorPosMultiple) {
239 4 : motorPosMsg.name.push_back(motorPos.first);
240 4 : motorPosMsg.position.push_back(motorPos.second.at(0)); //at(0). because multiple dof is considered in general
241 :
242 : }
243 4 : graspingMsg->action_motor_positions.push_back(motorPosMsg);
244 : }
245 :
246 8 : for (auto elementInvolved : action->getFingersInvolved()) {
247 4 : graspingMsg->elements_involved.push_back(elementInvolved);
248 : }
249 4 : }
250 :
251 0 : bool ROSEE::RosServiceHandler::primitiveAggregatedCallback(
252 : rosee_msg::GraspingPrimitiveAggregatedAvailable::Request& request,
253 : rosee_msg::GraspingPrimitiveAggregatedAvailable::Response& response) {
254 :
255 0 : if (request.primitive_type == 0) {
256 :
257 0 : if (request.action_name.size() == 0 ) {
258 : // return all primitives
259 :
260 0 : for (auto primitiveMaps : _mapActionHandler->getAllPrimitiveMaps() ) {
261 :
262 0 : response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitiveMaps.second));
263 : }
264 :
265 : } else {
266 0 : if (request.elements_involved.size() == 0) {
267 :
268 0 : auto primitiveMap = _mapActionHandler->getPrimitiveMap(request.action_name);
269 0 : response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitiveMap));
270 :
271 :
272 : } else {
273 :
274 0 : auto primitive =_mapActionHandler->getPrimitive(request.action_name, request.elements_involved);
275 0 : response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitive));
276 : }
277 : }
278 :
279 : } else {
280 :
281 0 : 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 0 : for (auto primitiveMap : _mapActionHandler->getPrimitiveMap(
285 0 : static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1))) {
286 :
287 0 : response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitiveMap));
288 :
289 : }
290 :
291 : } else {
292 :
293 0 : for (auto primitive : _mapActionHandler->getPrimitive(
294 0 : static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1), request.elements_involved)) {
295 :
296 0 : response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitive));
297 : }
298 : }
299 : }
300 0 : return true;
301 : }
302 :
303 :
304 0 : rosee_msg::GraspingPrimitiveAggregated ROSEE::RosServiceHandler::fillPrimitiveAggregatedMsg(
305 : ROSEE::MapActionHandler::ActionPrimitiveMap primitiveMap) {
306 :
307 0 : rosee_msg::GraspingPrimitiveAggregated primitiveMsg;
308 :
309 0 : primitiveMsg.action_name = primitiveMap.begin()->second->getName();
310 0 : 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 0 : primitiveMsg.max_selectable = primitiveMap.begin()->first.size();
315 : //TODO extract the keys with another mapActionHandler function?
316 0 : primitiveMsg.selectable_names =
317 0 : ROSEE::Utils::extract_keys_merged(primitiveMap, nFinger);
318 :
319 0 : return primitiveMsg;
320 : }
321 :
322 0 : rosee_msg::GraspingPrimitiveAggregated ROSEE::RosServiceHandler::fillPrimitiveAggregatedMsg(
323 : ROSEE::ActionPrimitive::Ptr primitive) {
324 :
325 0 : rosee_msg::GraspingPrimitiveAggregated primitiveMsg;
326 0 : primitiveMsg.action_name = primitive->getName();
327 0 : primitiveMsg.primitive_type = primitive->getPrimitiveType();
328 :
329 0 : auto elements = primitive->getKeyElements();
330 0 : primitiveMsg.max_selectable = elements.size();
331 0 : primitiveMsg.selectable_names.assign(elements.begin(), elements.end());
332 :
333 0 : return primitiveMsg;
334 : }
335 :
336 :
337 0 : bool ROSEE::RosServiceHandler::selectablePairInfoCallback(
338 : rosee_msg::SelectablePairInfo::Request& request,
339 : rosee_msg::SelectablePairInfo::Response& response) {
340 :
341 0 : std::set<std::string> companionFingers;
342 0 : if (request.action_name.compare ("pinchTight") == 0) {
343 0 : companionFingers =
344 0 : _mapActionHandler->getFingertipsForPinch(request.element_name,
345 : ROSEE::ActionPrimitive::Type::PinchTight) ;
346 :
347 0 : } else if (request.action_name.compare ("pinchLoose") == 0) {
348 0 : companionFingers =
349 0 : _mapActionHandler->getFingertipsForPinch(request.element_name,
350 : ROSEE::ActionPrimitive::Type::PinchLoose) ;
351 :
352 : } else {
353 0 : ROS_ERROR_STREAM ( "Received" << request.action_name << " that is not" <<
354 : "a recognizible action name to look for finger companions" );
355 0 : return false;
356 : }
357 :
358 0 : if (companionFingers.size() == 0) {
359 0 : return false;
360 : }
361 :
362 : //push the elements of set into the vector
363 0 : for (auto fing : companionFingers ) {
364 0 : response.pair_elements.push_back (fing);
365 : }
366 :
367 0 : return true;
368 : }
369 :
370 0 : bool ROSEE::RosServiceHandler::handInfoCallback(
371 : rosee_msg::HandInfo::Request& request,
372 : rosee_msg::HandInfo::Response& response) {
373 :
374 0 : if (! ros::service::exists("/EEHalExecutor/hand_info", false) ) {
375 0 : return false;
376 : }
377 :
378 : ros::ServiceClient handInfoClient =
379 0 : _nh->serviceClient<rosee_msg::HandInfo>("/EEHalExecutor/hand_info");
380 0 : rosee_msg::HandInfo handInfoMsg;
381 :
382 : //request may be empty or not according to which hal we are using
383 0 : handInfoMsg.request = request;
384 0 : if (handInfoClient.call(handInfoMsg)) {
385 :
386 0 : response = handInfoMsg.response;
387 :
388 : } else {
389 0 : return false;
390 : }
391 :
392 0 : return true;
393 : }
394 :
395 : //TODO error msg useless becaus if return false the response is not send
396 : //at today (2020) it seems there not exist a method to return false plus an error message.
397 28 : bool ROSEE::RosServiceHandler::newGraspingActionCallback(
398 : rosee_msg::NewGenericGraspingActionSrv::Request& request,
399 : rosee_msg::NewGenericGraspingActionSrv::Response& response){
400 :
401 28 : response.accepted = false;
402 28 : response.emitted = false;
403 :
404 28 : if (request.newAction.action_name.empty()) {
405 :
406 4 : response.error_msg = "action_name can not be empty";
407 4 : ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
408 4 : return true; //so the client receive the response
409 : }
410 :
411 68 : if (request.newAction.action_motor_position.name.size() == 0 ||
412 44 : request.newAction.action_motor_position.position.size() == 0 ||
413 20 : request.newAction.action_motor_position.position.size() != request.newAction.action_motor_position.name.size()) {
414 :
415 4 : response.error_msg = "action_motor_position is empty or badly formed";
416 4 : ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
417 :
418 4 : return true; //so the client receive the response
419 : }
420 :
421 20 : if (request.newAction.action_motor_count.name.size() != request.newAction.action_motor_count.count.size()) {
422 :
423 0 : response.error_msg = "action_motor_count is badly formed, name and count have different sizes";
424 0 : ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
425 :
426 0 : 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 20 : if (_mapActionHandler->getGeneric(request.newAction.action_name, false) != nullptr) {
433 :
434 4 : response.error_msg = "A generic action with name '" + request.newAction.action_name + "' already exists";
435 4 : ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
436 :
437 4 : return true; //so the client receive the response
438 :
439 : }
440 :
441 32 : ROSEE::ActionGeneric::Ptr newAction;
442 32 : ROSEE::JointPos jp;
443 32 : ROSEE::JointsInvolvedCount jic;
444 32 : std::set<std::string> elementInvolved;
445 :
446 32 : for (int i = 0; i < request.newAction.action_motor_position.name.size(); i++){
447 :
448 32 : std::vector<double> one_dof{request.newAction.action_motor_position.position.at(i)};
449 16 : jp.insert(std::make_pair(request.newAction.action_motor_position.name.at(i),
450 : one_dof));
451 : }
452 :
453 24 : for (int i = 0; i < request.newAction.action_motor_count.name.size(); i++){
454 :
455 8 : jic.insert(std::make_pair(request.newAction.action_motor_count.name.at(i),
456 8 : request.newAction.action_motor_count.count.at(i)));
457 : }
458 :
459 20 : for (int i = 0; i< request.newAction.elements_involved.size(); i++) {
460 :
461 4 : elementInvolved.insert(request.newAction.elements_involved.at(i));
462 : }
463 :
464 : //costructor will handle jpc and elementInvolved also if empty
465 16 : try { newAction = std::make_shared<ROSEE::ActionGeneric>(request.newAction.action_name,
466 : jp,
467 : jic,
468 : elementInvolved);
469 :
470 8 : } catch (const ROSEE::Utils::DifferentKeysException<ROSEE::JointPos, ROSEE::JointsInvolvedCount>) {
471 :
472 4 : 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 4 : ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
474 :
475 4 : 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 12 : if (! _mapActionHandler->insertSingleGeneric(newAction)){
481 :
482 0 : response.error_msg = "error by mapActionHandler when inserting the new generic action";
483 0 : ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
484 :
485 0 : return true; //so the client receive the response
486 : }
487 :
488 12 : ROS_INFO_STREAM ( "[RosServiceHandler " << __func__ << " ] The new action '"<< newAction->getName() << "' is inserted in the system");
489 :
490 :
491 12 : if (request.emitYaml) {
492 :
493 4 : ROSEE::YamlWorker yamlWorker;
494 8 : auto path = yamlWorker.createYamlFile(newAction, _path2saveYamlGeneric);
495 4 : ROS_INFO_STREAM ( "[RosServiceHandler " << __func__ << " ] The received new action '"<< newAction->getName() << "' has been stored in " << path);
496 4 : response.emitted = true;
497 : }
498 :
499 12 : response.accepted = true;
500 12 : return true;
501 123 : }
|
