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.

                                                                                                                                       {
    
    if (mapActionHandler == nullptr) {
        ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] the mapActionHandler in not initialized");
        return;
    }
    
    this->_mapActionHandler = mapActionHandler;
    this->_path2saveYamlGeneric = path2saveYamlGeneric;

    this->_nh = nh;
    
}

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

action	the action that must be sent as response
msg	the ROS message that must be filled with action info

Definition at line 221 of file RosServiceHandler.cpp.

                                                                                                     {
    
    graspingMsg->action_type = action->getType();
    graspingMsg->action_name = action->getName();
    
    rosee_msg::JointsInvolvedCount motorCountMsg;
    for ( auto motorCount : action->getJointsInvolvedCount()) { 
        motorCountMsg.name.push_back(motorCount.first);
        motorCountMsg.count.push_back(motorCount.second);
    }
    graspingMsg->action_motor_count = motorCountMsg;
    
    for ( auto motorPosMultiple : action->getAllJointPos()) {

        //iterate over the single motor positions
        rosee_msg::MotorPosition motorPosMsg;
        for ( auto motorPos : motorPosMultiple) { 
            motorPosMsg.name.push_back(motorPos.first);
            motorPosMsg.position.push_back(motorPos.second.at(0)); //at(0). because multiple dof is considered in general

        }
        graspingMsg->action_motor_positions.push_back(motorPosMsg); 
    }
    
    for (auto elementInvolved : action->getFingersInvolved()) {
        graspingMsg->elements_involved.push_back(elementInvolved);
    }
}

rosee_msg::GraspingAction ROSEE::RosServiceHandler::fillGraspingActionMsg ( ROSEE::ActionPrimitive::Ptr  primitive )

private

Definition at line 163 of file RosServiceHandler.cpp.

                                                                                                       {
    
    rosee_msg::GraspingAction primitiveMsg;
    
    if (primitive == nullptr) {
        return primitiveMsg;
    }

    fillCommonInfoGraspingActionMsg(primitive, &primitiveMsg);

    primitiveMsg.primitive_type = primitive->getPrimitiveType();
    
    auto elements = primitive->getKeyElements();
    primitiveMsg.elements_involved.assign(elements.begin(), elements.end());
    
    return primitiveMsg;
}

rosee_msg::GraspingAction ROSEE::RosServiceHandler::fillGraspingActionMsg ( ROSEE::ActionGeneric::Ptr  generic )

private

Definition at line 181 of file RosServiceHandler.cpp.

                                                                                                   {
    
    rosee_msg::GraspingAction genericActionMsg;
    if (generic == nullptr) {
        return genericActionMsg;
    }
    
    fillCommonInfoGraspingActionMsg(generic, &genericActionMsg);

    genericActionMsg.primitive_type = genericActionMsg.PRIMITIVE_NONE;
    
    ActionComposed::Ptr composedCasted = std::dynamic_pointer_cast<ActionComposed>(generic);
    if ( composedCasted != nullptr) {
        genericActionMsg.inner_actions = composedCasted->getInnerActionsNames();
    }
    
    return genericActionMsg;
}

rosee_msg::GraspingAction ROSEE::RosServiceHandler::fillGraspingActionMsg ( ROSEE::ActionTimed::Ptr  timed )

private

Definition at line 200 of file RosServiceHandler.cpp.

                                                                                               {
    
    rosee_msg::GraspingAction timedActionMsg;
    if (timed == nullptr) {
        return timedActionMsg;
    }
    
    fillCommonInfoGraspingActionMsg(timed, &timedActionMsg);

    timedActionMsg.primitive_type = timedActionMsg.PRIMITIVE_NONE;
    
    timedActionMsg.inner_actions = timed->getInnerActionsNames();

    for (auto innerMargin : timed->getAllActionMargins()){
        timedActionMsg.before_time_margins.push_back(innerMargin.first);
        timedActionMsg.after_time_margins.push_back(innerMargin.second);
    }
    
    return timedActionMsg;
}

rosee_msg::GraspingPrimitiveAggregated ROSEE::RosServiceHandler::fillPrimitiveAggregatedMsg ( ROSEE::MapActionHandler::ActionPrimitiveMap  primitiveMap )

private

Definition at line 304 of file RosServiceHandler.cpp.

                                                          {
    
    rosee_msg::GraspingPrimitiveAggregated primitiveMsg;

    primitiveMsg.action_name = primitiveMap.begin()->second->getName();
    primitiveMsg.primitive_type = primitiveMap.begin()->second->getPrimitiveType();
    //until now, there is not a primitive that does not have "something" to select
    // (eg pinch has 2 fing, trig one fing, singleJointMultipleTips 1 joint...). 
    //Instead generic action has always no thing to select (next for loop)
    primitiveMsg.max_selectable = primitiveMap.begin()->first.size();
    //TODO extract the keys with another mapActionHandler function?
    primitiveMsg.selectable_names =
        ROSEE::Utils::extract_keys_merged(primitiveMap, nFinger);

    return primitiveMsg;
}

rosee_msg::GraspingPrimitiveAggregated ROSEE::RosServiceHandler::fillPrimitiveAggregatedMsg ( ROSEE::ActionPrimitive::Ptr  primitive )

private

Definition at line 322 of file RosServiceHandler.cpp.

                                       {

    rosee_msg::GraspingPrimitiveAggregated primitiveMsg;
    primitiveMsg.action_name = primitive->getName();
    primitiveMsg.primitive_type = primitive->getPrimitiveType();
    
    auto elements = primitive->getKeyElements();
    primitiveMsg.max_selectable = elements.size();
    primitiveMsg.selectable_names.assign(elements.begin(), elements.end());

    return primitiveMsg;
}

bool ROSEE::RosServiceHandler::graspingActionsCallback ( rosee_msg::GraspingActionsAvailable::Request &  request, rosee_msg::GraspingActionsAvailable::Response &  response  )

private

Definition at line 65 of file RosServiceHandler.cpp.

                                                         {
    
    switch (request.action_type) {
        
        case 0 : { //PRIMITIVE
            
            //NOTE if both primitive type and action name are set in the request, the action name is not considered

            if (request.primitive_type == 0) { 
                
                if (request.action_name.size() == 0 ) {            
                    for (auto primitiveContainers : _mapActionHandler->getAllPrimitiveMaps() ) {
                        
                        //iterate all the primitive of a type
                        for (auto primitive : primitiveContainers.second) {
                            response.grasping_actions.push_back(fillGraspingActionMsg(primitive.second));
                        }
                    }
                    
                } else {
                    if (request.elements_involved.size() == 0) {
                        
                        for (auto primitive : _mapActionHandler->getPrimitiveMap(request.action_name)) {
                            response.grasping_actions.push_back(fillGraspingActionMsg(primitive.second));
                        }
                        
                    } else {
        
                        auto primitive =_mapActionHandler->getPrimitive(request.action_name, request.elements_involved);
                        response.grasping_actions.push_back(fillGraspingActionMsg(primitive));
                    }
                }
                
            } else {
               
                if (request.elements_involved.size() == 0) {
                        
                    //NOTE -1 because in the srv 0 is for all primitives, then the enum is scaled by one
                    for (auto primitives : _mapActionHandler->getPrimitiveMap(
                            static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1))) {
                        
                        for (auto primitive : primitives) {
                            response.grasping_actions.push_back(fillGraspingActionMsg(primitive.second));
                        }
                    }
                              
                } else {
                     
                    for (auto primitive : _mapActionHandler->getPrimitive(
                            static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1), request.elements_involved)) {
                        
                            response.grasping_actions.push_back(fillGraspingActionMsg(primitive));
                    }
                }
            }
            
            break;
        }
        
        case 1 : { //GENERIC_and_COMPOSED
            
            //NOTE for these some fields are ignored 
            if (request.action_name.size() == 0) {
                for (auto action : _mapActionHandler->getAllGenerics()) {
                    response.grasping_actions.push_back(fillGraspingActionMsg(action.second));
                }
                
            } else {
                response.grasping_actions.push_back(fillGraspingActionMsg(_mapActionHandler->getGeneric(request.action_name)));
            }
            break;
        }
        
        case 2 : { //TIMED
            if (request.action_name.size() == 0) {
                for (auto action : _mapActionHandler->getAllTimeds()) {
                    response.grasping_actions.push_back(fillGraspingActionMsg(action.second));
                }
                
            } else {
                response.grasping_actions.push_back(fillGraspingActionMsg(_mapActionHandler->getTimed(request.action_name)));
            }
            break;
        }
        
        default : {
            ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] request.actionType can only be 0(ALL), 1(PRIMITIVE), "
                << "2(GENERIC_and_COMPOSED), or 3(TIMED); I have received " << request.action_type);
            return false;
            
        }
    }
    
    return true;    
}

bool ROSEE::RosServiceHandler::handInfoCallback ( rosee_msg::HandInfo::Request &  request, rosee_msg::HandInfo::Response &  response  )

private

Definition at line 370 of file RosServiceHandler.cpp.

                                             {

    if (! ros::service::exists("/EEHalExecutor/hand_info", false) ) {
        return false;
    }
    
    ros::ServiceClient handInfoClient = 
        _nh->serviceClient<rosee_msg::HandInfo>("/EEHalExecutor/hand_info");
    rosee_msg::HandInfo handInfoMsg;
    
    //request may be empty or not according to which hal we are using
    handInfoMsg.request = request;
    if (handInfoClient.call(handInfoMsg)) {
        
        response = handInfoMsg.response;
        
    } else {
        return false;
    }
    
    return true;
}

bool ROSEE::RosServiceHandler::init

(

unsigned int

nFinger

)

Definition at line 34 of file RosServiceHandler.cpp.

                                                      {
    
    this->nFinger = nFinger;
    
    std::string graspingActionsSrvName, actionInfoServiceName, 
      selectablePairInfoServiceName, handInfoServiceName, newGraspingActionServiceName;

    _nh->param<std::string>("/rosee/grasping_action_srv_name", graspingActionsSrvName, "grasping_actions_available");
    _nh->param<std::string>("/rosee/primitive_aggregated_srv_name", actionInfoServiceName, "primitives_aggregated_available");
    _nh->param<std::string>("/rosee/selectable_finger_pair_info", selectablePairInfoServiceName, "selectable_finger_pair_info");
    _nh->param<std::string>("/rosee/hand_info", handInfoServiceName, "hand_info");
    _nh->param<std::string>("/rosee/new_grasping_action_srv_name", newGraspingActionServiceName, "new_generic_grasping_action");
    
    _serverGraspingActions = _nh->advertiseService(graspingActionsSrvName, 
        &RosServiceHandler::graspingActionsCallback, this);

    _serverPrimitiveAggregated = _nh->advertiseService(actionInfoServiceName, 
        &RosServiceHandler::primitiveAggregatedCallback, this);
    
    _server_selectablePairInfo = _nh->advertiseService(selectablePairInfoServiceName, 
        &RosServiceHandler::selectablePairInfoCallback, this);
    
    _serverHandInfo = _nh->advertiseService(handInfoServiceName, 
        &RosServiceHandler::handInfoCallback, this);

    _serverNewGraspingAction = _nh->advertiseService(newGraspingActionServiceName, 
        &RosServiceHandler::newGraspingActionCallback, this);
    
    return true;
}

bool ROSEE::RosServiceHandler::newGraspingActionCallback ( rosee_msg::NewGenericGraspingActionSrv::Request &  request, rosee_msg::NewGenericGraspingActionSrv::Response &  response  )

private

Definition at line 397 of file RosServiceHandler.cpp.

                                                               {
    
    response.accepted = false;
    response.emitted = false;
    
    if (request.newAction.action_name.empty()) {
        
        response.error_msg = "action_name can not be empty";
        ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);
        return true; //so the client receive the response
    }
    
    if (request.newAction.action_motor_position.name.size() == 0 ||
        request.newAction.action_motor_position.position.size() == 0 ||
        request.newAction.action_motor_position.position.size() != request.newAction.action_motor_position.name.size()) {
        
        response.error_msg = "action_motor_position is empty or badly formed";
        ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);

        return true; //so the client receive the response
    }
    
    if (request.newAction.action_motor_count.name.size() != request.newAction.action_motor_count.count.size()) {
        
        response.error_msg = "action_motor_count is badly formed, name and count have different sizes";
        ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);

        return true; //so the client receive the response
    }
    
    // TODO request.newAction.action_motor_count : if empty, ActionGeneric costructor will consider all joint
    // with 0 position as not used. This may change in future when we will support not 0 default joint positions
    
    if (_mapActionHandler->getGeneric(request.newAction.action_name, false) != nullptr) {
        
        response.error_msg = "A generic action with name '" + request.newAction.action_name + "' already exists";
        ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);

        return true; //so the client receive the response
        
    }

    ROSEE::ActionGeneric::Ptr newAction;
    ROSEE::JointPos jp;
    ROSEE::JointsInvolvedCount jic;
    std::set<std::string> elementInvolved;
    
    for (int i = 0; i < request.newAction.action_motor_position.name.size(); i++){
        
        std::vector<double> one_dof{request.newAction.action_motor_position.position.at(i)};        
        jp.insert(std::make_pair(request.newAction.action_motor_position.name.at(i),
                                 one_dof));
    }
    
    for (int i = 0; i < request.newAction.action_motor_count.name.size(); i++){
        
        jic.insert(std::make_pair(request.newAction.action_motor_count.name.at(i),
                                  request.newAction.action_motor_count.count.at(i)));
    }
    
    for (int i = 0; i< request.newAction.elements_involved.size(); i++) {
        
        elementInvolved.insert(request.newAction.elements_involved.at(i));  
    }
    
    //costructor will handle jpc and elementInvolved also if empty
    try { newAction = std::make_shared<ROSEE::ActionGeneric>(request.newAction.action_name,
                                                             jp,
                                                             jic,
                                                             elementInvolved);
    
    } catch (const ROSEE::Utils::DifferentKeysException<ROSEE::JointPos, ROSEE::JointsInvolvedCount>) {
        
        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";
        ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);

        return true; //so the client receive the response
    } 
    
    //u rosee main node use always mapActionHandler to check if an action exists. Thus, we need to add this new 
    // action into the mapActionHandler "database" (ie private member map of the generic actions)
    if (! _mapActionHandler->insertSingleGeneric(newAction)){
        
        response.error_msg = "error by mapActionHandler when inserting the new generic action";
        ROS_ERROR_STREAM ( "[RosServiceHandler " << __func__ << " ] " << response.error_msg);

        return true; //so the client receive the response
    }
    
    ROS_INFO_STREAM ( "[RosServiceHandler " << __func__ << " ] The new action '"<< newAction->getName() << "' is inserted in the system");

    
    if (request.emitYaml) {
        
        ROSEE::YamlWorker yamlWorker;
        auto path = yamlWorker.createYamlFile(newAction, _path2saveYamlGeneric);
        ROS_INFO_STREAM ( "[RosServiceHandler " << __func__ << " ] The received new action '"<< newAction->getName() << "' has been stored in " << path);
        response.emitted = true;
    }
    
    response.accepted = true;
    return true;
}

bool ROSEE::RosServiceHandler::primitiveAggregatedCallback ( rosee_msg::GraspingPrimitiveAggregatedAvailable::Request &  request, rosee_msg::GraspingPrimitiveAggregatedAvailable::Response &  response  )

private

Definition at line 251 of file RosServiceHandler.cpp.

                                                                     {
    
     if (request.primitive_type == 0) { 
                
        if (request.action_name.size() == 0 ) {
            // return all primitives
                        
            for (auto primitiveMaps : _mapActionHandler->getAllPrimitiveMaps() ) {
                
                response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitiveMaps.second));
            }
            
        } else {
            if (request.elements_involved.size() == 0) {
                
                auto primitiveMap = _mapActionHandler->getPrimitiveMap(request.action_name);                     
                response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitiveMap));

                
            } else {

                auto primitive =_mapActionHandler->getPrimitive(request.action_name, request.elements_involved);
                response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitive));
            }
        }
        
    } else {
        
        if (request.elements_involved.size() == 0) {
                
            //NOTE -1 because in the srv 0 is for all primitives, then the enum is scaled by one
            for (auto primitiveMap : _mapActionHandler->getPrimitiveMap(
                    static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1))) {
            
                response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitiveMap));

            }
                        
        } else {
                
            for (auto primitive : _mapActionHandler->getPrimitive(
                    static_cast<ROSEE::ActionPrimitive::Type>(request.primitive_type-1), request.elements_involved)) {
                                    
                response.primitives_aggregated.push_back(fillPrimitiveAggregatedMsg(primitive));
            }
        }
    }
    return true;    
}

bool ROSEE::RosServiceHandler::selectablePairInfoCallback ( rosee_msg::SelectablePairInfo::Request &  request, rosee_msg::SelectablePairInfo::Response &  response  )

private

Definition at line 337 of file RosServiceHandler.cpp.

                                                   {
    
    std::set<std::string> companionFingers;
    if (request.action_name.compare ("pinchTight") == 0) {
        companionFingers =
            _mapActionHandler->getFingertipsForPinch(request.element_name,
                ROSEE::ActionPrimitive::Type::PinchTight) ;
                
    } else if (request.action_name.compare ("pinchLoose") == 0) {
        companionFingers =
            _mapActionHandler->getFingertipsForPinch(request.element_name,
                ROSEE::ActionPrimitive::Type::PinchLoose) ;
                
    } else {
        ROS_ERROR_STREAM ( "Received" << request.action_name << " that is not" <<
            "a recognizible action name to look for finger companions" );
        return false;
    }
    
    if (companionFingers.size() == 0) {
        return false;
    }
    
    //push the elements of set into the vector
    for (auto fing : companionFingers ) {
        response.pair_elements.push_back (fing);
    }
    
    return true;        
}

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:

• /home/travis/build/ADVRHumanoids/ROSEndEffector/include/end_effector/RosServiceHandler.h
• /home/travis/build/ADVRHumanoids/ROSEndEffector/src/RosServiceHandler.cpp