ArmComponentsNameManager.h

Go to the documentation of this file.

#ifndef ARM_COMPONENTS_NAME_MANAGER_ARMCOMPONENTSNAMEMANAGER_H
#define ARM_COMPONENTS_NAME_MANAGER_ARMCOMPONENTSNAMEMANAGER_H

#ifdef DOXYGEN_SHOULD_SKIP_THIS

#endif

#include <string>
#include <vector>
#include <sensor_msgs/JointState.h>
#include <ros/ros.h>

namespace arm_components_name_manager
{

class ArmComponentsNameManager
{
public:
    ArmComponentsNameManager(const std::string& robot_namespace, bool readParams);
    ArmComponentsNameManager(const ArmComponentsNameManager& o);

    int loadParameters(bool printErrors=true, bool verbose=false);

    bool waitToLoadParameters(int sufficientSuccessCode, float maxWait, float waitStep=0.1);

    bool loadDefaults();

    int parametersLoaded()
    {
        return initParamCode;
    }

    bool defaultsLoaded()
    {
        return initWithDefaults;
    }

    void getJointNames(std::vector<std::string>& joint_names,
                       bool withGripper, const std::string& prepend = std::string()) const;

    const std::vector<std::string>& getArmJoints() const;

    const std::vector<std::string>& getGripperJoints() const;

    const std::vector<float>& getArmJointsInitPose() const;

    const std::vector<float>& getGripperJointsInitPose() const;

    const std::vector<std::string>& getGripperLinks() const;

    const std::string& getPalmLink() const;
    
    const std::string& getEffectorLink() const;

    const std::vector<std::string>& getArmLinks() const;


//    void initJointState(sensor_msgs::JointState& js, bool withGripper = true,
//                        const std::vector<float> * init_poses = NULL) const;

    void copyToJointState(sensor_msgs::JointState& js, int mode, const std::vector<float> * init_vals, int copyData, bool resetAll) const;


    bool extractFromJointState(const sensor_msgs::JointState& js, int mode, std::vector<float>& data, int extractData) const;
    bool extractFromJointState(const sensor_msgs::JointState& js, int mode, sensor_msgs::JointState& result) const;

    int getJointIndices(const std::vector<std::string>& joint_names, std::vector<int>& idx) const;

    bool getJointIndices(const std::vector<std::string>& joint_names, std::vector<int>& idx, int mode) const;

    int numArmJoints() const;
    int numGripperJoints() const;
    int numTotalJoints() const
    {
        return numArmJoints() + numGripperJoints();
    }
    bool isGripper(const std::string& name) const;

    int armJointNumber(const std::string& name) const;

    int gripperJointNumber(const std::string& name) const;

    void ReadPIDValues(const std::string& pidParameterName, float& kp, float& ki, float& kd) const;

    bool GetPosGains(const std::string& jointName, float& kp, float& ki, float& kd) const;

    bool GetVelGains(const std::string& jointName, float& kp, float& ki, float& kd) const;


    virtual bool GetMaxVals(const std::string& jointName, float& force, float& velocity) const;


    void setValues(const std::string& _palm_link,
                   const std::string& _effector_link,
                   const std::vector<std::string>& _arm_joints, const std::vector<std::string>& _arm_links,
                   const std::vector<std::string>& _gripper_joints, const std::vector<std::string>& _gripper_links,
                   const std::vector<float>& _arm_joint_init, const std::vector<float>& _gripper_joint_init);

    void setControllerNames(const std::vector<std::string>& controller_names, bool forArm, int type);

    virtual bool hasDefaults()
    {
        return false;
    }

protected:

    virtual std::string getDefaultPalmLink() const { return std::string(); }
    virtual std::string getDefaultEffectorLink() const { return std::string(); }
    virtual std::vector<std::string> getDefaultArmJoints() const { return std::vector<std::string>(); }
    virtual std::vector<std::string> getDefaultArmLinks() const { return std::vector<std::string>(); }
    virtual std::vector<std::string> getDefaultGripperJoints() const { return std::vector<std::string>(); }
    virtual std::vector<std::string> getDefaultGripperLinks() const { return std::vector<std::string>(); }
    virtual std::vector<float> getDefaultArmJointsInitPose() const { return std::vector<float>(); }
    virtual std::vector<float> getDefaultGripperJointsInitPose() const { return std::vector<float>(); }
    virtual std::vector<float> getDefaultArmJointsMaxVel() const { return std::vector<float>(); }
    virtual std::vector<float> getDefaultArmJointsMaxForce() const { return std::vector<float>(); }
    virtual std::vector<float> getDefaultGripperJointsMaxVel() const { return std::vector<float>(); }
    virtual std::vector<float> getDefaultGripperJointsMaxForce() const { return std::vector<float>(); }

private:

    ArmComponentsNameManager();

    std::vector<std::string> arm_joints;

    std::vector<std::string> arm_links;

    std::vector<std::string> gripper_joints;


    std::vector<std::string> gripper_links;

    std::string palm_link;
    
    std::string effector_link;

    std::vector<float> arm_joint_init;

    std::vector<float> gripper_joint_init;
   
    std::vector<float> arm_joint_max_vel;
    std::vector<float> arm_joint_max_force;
    std::vector<float> gripper_joint_max_vel;
    std::vector<float> gripper_joint_max_force;

    std::vector<std::string> arm_effort_controller_names;
    std::vector<std::string> arm_velocity_controller_names;
    std::vector<std::string> arm_position_controller_names;

    std::vector<std::string> gripper_effort_controller_names;
    std::vector<std::string> gripper_velocity_controller_names;
    std::vector<std::string> gripper_position_controller_names;

    std::string robot_namespace;

    int initParamCode;

    bool initWithDefaults;
};
}  // namespace
#endif  // ARM_COMPONENTS_NAME_MANAGER_ARMCOMPONENTSNAMEMANAGER_H