GripperSupervisor.h

Go to the documentation of this file.

#ifndef GRIPPER_SUPERVISOR_H
#define GRIPPER_SUPERVISOR_H

#include <kdl/tree.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/frames.hpp>

#include "r2_msgs/GripperPositionState.h"
#include "r2_msgs/StringArray.h"
#include "r2_msgs/GripperPositionCommand.h"
#include "r2_msgs/JointControlDataArray.h"
#include "actionlib_msgs/GoalStatusArray.h"
#include "robodyn_utilities/BaseGripperConverter.h"

struct EndEffectorData
{
    bool locked;
    bool loaded;
    bool verified;
};

enum GripperSupervisorStates
{
    IDLE = 0,
    FAULT,
    FAILURE,
    LOCK,
    RELEASE,
    SET,
    RELEASE_PENDING,
    SET_PENDING,
    LOCK_PENDING,
    REQUEST_PENDING
};

struct GripperControlModes
{
    int state;
    int coeffs;
};

class GripperSupervisor : public BaseGripperConverter
{
public:
    GripperSupervisor();
    ~GripperSupervisor();

    // intitialize
    void initializeMaps(const std::vector<std::string>& grippers);

    // put data into maps
    void populateGripperStateInfo(const r2_msgs::GripperPositionState& eefState);
    void populateBaseFrameInfo(const r2_msgs::StringArray& baseFrames);
    void populateGoalStatus(const actionlib_msgs::GoalStatusArray& goalStats);
    void populateGripperCommandsIn(const r2_msgs::GripperPositionCommand& gripCmd);
    void populateJointControlMode(const r2_msgs::JointControlDataArray& jntMode);
    void populateCheckedGrippers(const bool override);
    void populateCheckedGrippers(const r2_msgs::StringArray& grippers);

    // create messages from data
    void populateDriveRequestMsg(r2_msgs::StringArray& requestOut);
    void populateJointControlCommandMsg(r2_msgs::JointControlDataArray& modeCmdOut);
    void populateGoalStatusMsg(actionlib_msgs::GoalStatusArray& statusOut);
    void populateGripperCommandMsg(r2_msgs::GripperPositionCommand& cmdOut);

    // do the work
    void checkTransitions(int baseOverride);

    // access the data (mostly for unit testing)
    int getState(const std::string& name);

    // set limits (from component properties)
    void setLimits(const int& highDuty, const int& time);

    std::map<std::string, EndEffectorData> endEffectorMap;
    std::vector<std::string> baseFrameList;
    std::map<std::string, GripperControlModes> gripperJointModeMap;
    std::map<std::string, bool> gripperDriveApprovedMap;
    r2_msgs::GripperPositionCommand gripperCmds;
    actionlib_msgs::GoalStatusArray goalStatuses;
    r2_msgs::StringArray driveRequest;

private:

    actionlib_msgs::GoalStatusArray goalStatusesOut;
    r2_msgs::GripperPositionCommand gripperCmdOut;
    r2_msgs::JointControlDataArray jointModeCommands;

    std::map<std::string, bool> errorStates; // 1- error, 0- no error
    std::map<std::string, GripperSupervisorStates> stateMap;
    std::map<std::string, EndEffectorData>::iterator eefIt;
    std::map<std::string, GripperControlModes>::iterator intIt;
    std::map<std::string, GripperSupervisorStates>::iterator stateIt;
    std::map<std::string, bool>::iterator errIt, reqIt;

    std::string setpoint;
    int motcomLimit;
    int hiDutyCycleLimit;
    int timeLimit, pendingTimer;
    GripperSupervisorStates nextState;

    int detectChangeInErrorState(const std::string& name);
    int updateErrorState(const std::string& name);
    bool isReleaseValid(const std::string& name);
    bool isLocked(const std::string& name);

    bool transToFault(const std::string& name);
    bool transToIdle(const std::string& name);
    bool transToFailure(const std::string& name);
    bool transToLock(const std::string& name);
    bool transToRelease(const std::string& name);
    bool transToSet(const std::string& name);

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

};

#endif