TaskYarpInterface.h

Go to the documentation of this file.

#ifndef TASK_YARP_INTERFACE_H
#define TASK_YARP_INTERFACE_H


#include "ocra/util/Macros.h"
#include "ocra/util/ErrorsHelper.h"
#include "ocra/control/Task.h"
#include "ocra/control/TaskState.h"
#include "ocra/control/TaskYarpInterfaceVocab.h"
#include "ocra/control/Trajectory/Trajectories.h"
#include "ocra/util/ErrorsHelper.h"
#include <Eigen/Dense>

#include <yarp/os/Network.h>
#include <yarp/os/PortReader.h>
#include <yarp/os/RpcServer.h>
#include <yarp/os/ConnectionReader.h>
#include <yarp/os/Port.h>
#include <yarp/os/RateThread.h>
#include <yarp/os/Log.h>
#include <yarp/os/LogStream.h>

namespace ocra
{

enum TASK_MODE
{
    TASK_AS_OBJECTIVE,
    TASK_AS_CONSTRAINT,
    TASK_NOT_DEFINED
};


class TaskYarpInterface
{
    DEFINE_CLASS_POINTER_TYPEDEFS(TaskYarpInterface)
public:

    TaskYarpInterface(Task::Ptr taskPtr);

    virtual ~TaskYarpInterface();

    bool activate();

    bool activate(const TASK_MODE tmode);

    bool deactivate();

    TASK_MODE getTaskMode();

    std::string getPortName();

    bool isActivated();

    Eigen::VectorXd getTaskError();
    int getTaskHierarchyLevel();
    void setTaskHierarchyLevel(int level);
    double getTaskErrorNorm();

    void setStiffness(double K);

    void setStiffness(const VectorXd& K);


    void setStiffness(const MatrixXd& K);

    double getStiffness();

    Eigen::MatrixXd getStiffnessMatrix();

    void setDamping(double B);

    void setDamping(const VectorXd& B);


    void setDamping(const MatrixXd& B);

    double getDamping();

    Eigen::MatrixXd getDampingMatrix();


    void setWeight(double weight);

    void setWeight(const Eigen::VectorXd& weights);

    Eigen::VectorXd getWeight();



    TaskState getTaskState();
    TaskState getDesiredTaskState();
    void setDesiredTaskState(const TaskState& newDesiredTaskState);
    void setDesiredTaskStateDirect(const TaskState& newDesiredTaskState);
    void publishTaskState();
private:
//     yarp::os::Mutex getTaskStateMutex;


public: /* Segment Frame Based Tasks */

    // TODO: Document...
    Eigen::Displacementd getTaskFrameDisplacement();
    Eigen::Twistd getTaskFrameVelocity();
    Eigen::Twistd getTaskFrameAcceleration();
    Eigen::Vector3d getTaskFramePosition();
    Eigen::Rotation3d getTaskFrameOrientation();
    Eigen::Vector3d getTaskFrameLinearVelocity();
    Eigen::Vector3d getTaskFrameAngularVelocity();
    Eigen::Vector3d getTaskFrameLinearAcceleration();
    Eigen::Vector3d getTaskFrameAngularAcceleration();




public: /*Nested callback classes */

    class RpcMessageCallback : public yarp::os::PortReader {
        DEFINE_CLASS_POINTER_TYPEDEFS(RpcMessageCallback)

    private:
        TaskYarpInterface& tmBase;

    public:
        RpcMessageCallback(TaskYarpInterface& tmBaseRef);

        virtual bool read(yarp::os::ConnectionReader& connection);
    };

    class ControlInputCallback : public yarp::os::PortReader {
        DEFINE_CLASS_POINTER_TYPEDEFS(ControlInputCallback)

    private:
        TaskYarpInterface& tmBase;
        yarp::os::Bottle input;

    public:
        ControlInputCallback(TaskYarpInterface& tmBaseRef);

        virtual bool read(yarp::os::ConnectionReader& connection);
    };

    class StateUpdateThread : public yarp::os::RateThread
    {
        DEFINE_CLASS_POINTER_TYPEDEFS(StateUpdateThread)

    private:
        TaskYarpInterface& tmBase;


    public:
        StateUpdateThread(int period, TaskYarpInterface& tmBaseRef);
        bool threadInit();
        void run();
        void threadRelease();
    };


private: /* Private methods */
    // For parsing and compiling yarp messages.
    void parseIncomingMessage(yarp::os::Bottle& input, yarp::os::Bottle& reply);
    std::string printValidMessageTags();
    bool openControlPorts();
    bool closeControlPorts();
    bool parseControlInput(yarp::os::Bottle& input);


private:
    Task::Ptr                   task;
    TASK_MODE                   taskMode;
    yarp::os::Log               yLog;

    yarp::os::Network           yarp;
    yarp::os::RpcServer         rpcPort;
    std::string                 portName;
    RpcMessageCallback::Ptr     rpcCallback;

    bool                        controlPortsOpen;

    yarp::os::Bottle            stateInBottle;
    std::string                 inputControlPortName;
    yarp::os::Port              inputControlPort;
    ControlInputCallback::Ptr   controlCallback;

    yarp::os::Bottle            stateOutBottle;
    std::string                 outputControlPortName;
    yarp::os::Port              outputControlPort;

    yarp::os::Bottle            desiredStateOutBottle;
    std::string                 desiredStateOutputPortName;
    yarp::os::Port              desiredStateOutputPort;




    StateUpdateThread::Ptr      stateThread;
    int numberOfOpenRequests;
    

    bool logMessages;

};

}

#endif // TASK_YARP_INTERFACE_H