ocra-wbi-plugins/ocra-icub-server/include/ocra-icub-server/Thread.h

Go to the documentation of this file.

/*
 *  This file is part of ocra-icub.
 *  Copyright (C) 2016 Institut des Systèmes Intelligents et de Robotique (ISIR)
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef OCRA_CONTROLLER_SERVER_THREAD_H
#define OCRA_CONTROLLER_SERVER_THREAD_H

#include <yarpWholeBodyInterface/yarpWholeBodyInterface.h>
#include <wbi/wbi.h>

#include <ocra-icub-server/IcubControllerServer.h>

#include <ocra-icub/Utilities.h>
#include <ocra/util/ErrorsHelper.h>

#include <yarp/os/Bottle.h>
#include <yarp/os/RpcServer.h>
#include <yarp/os/ConnectionReader.h>
#include <yarp/os/Time.h>

#include <sstream>
#include <string>

#include <iDynTree/Estimation/SimpleLeggedOdometry.h>


class OcraControllerOptions
{
// CLASS_POINTER_TYPEDEFS(OcraControllerOptions)

public: // Functions
    OcraControllerOptions();

    ~OcraControllerOptions();

public: // Variables
    int                     threadPeriod; 
    std::string             serverName; 
    std::string             robotName; 
    std::string             startupTaskSetPath; 
    std::string             startupSequence; 
    std::string             wbiConfigFilePath; 
    std::string             urdfModelPath; 
    bool                    runInDebugMode; 
    bool                    noOutputMode; 
    bool                    isFloatingBase; 
    bool                    useOdometry; 
    bool                    idleAnkles; 
    double                  idleAnkleTime; 
    bool                    maintainFinalPosture; 
    yarp::os::Property      yarpWbiOptions; 
    ocra_recipes::CONTROLLER_TYPE    controllerType; 
    ocra_recipes::SOLVER_TYPE    solver; 
    double wDdq;
    double wTau;
    double wFc;
};


class Thread: public yarp::os::RateThread
{
// CLASS_POINTER_TYPEDEFS(Thread)
public:
    Thread(OcraControllerOptions& controller_options, std::shared_ptr<wbi::wholeBodyInterface> wbi);

    virtual ~Thread();
    bool threadInit();
    void run();
    void threadRelease();

public:
    class ControllerRpcServerCallback : public yarp::os::PortReader
    {
    CLASS_POINTER_TYPEDEFS(ControllerRpcServerCallback)

    public:

        ControllerRpcServerCallback(Thread& threadRef);

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

    private:

        Thread& thread; 
    };

public:
    class DebugRpcServerCallback : public yarp::os::PortReader
    {
    CLASS_POINTER_TYPEDEFS(DebugRpcServerCallback)

    public:

        DebugRpcServerCallback(Thread& threadRef);

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

    private:

        Thread& thread; 
    };

private:
    ocra_icub::OCRA_ICUB_MESSAGE convertStringToOcraIcubMessage(const std::string& s);
    void parseIncomingMessage(yarp::os::Bottle& input, yarp::os::Bottle& reply);
    void parseDebugMessage(yarp::os::Bottle& input, yarp::os::Bottle& reply);
    void writeDebugData();
    void putAnklesIntoIdle(double idleTime);
    void sendTorqueReferenceToDebugJoint(int idx);
    bool setDebugJointToTorqueMode(int idx);

private:
    ocra::Model::Ptr model;
    std::shared_ptr<IcubControllerServer> ctrlServer;
    static const int ALL_JOINTS = -1; 
    //TODO: Need to get the torque mins and maxs at the joint level.
    static const int TORQUE_MIN = -24; 
    static const int TORQUE_MAX = 24; 
    Eigen::ArrayXd minTorques; 
    Eigen::ArrayXd maxTorques; 
    OcraControllerOptions ctrlOptions; 
    std::shared_ptr<wbi::wholeBodyInterface> yarpWbi; 
    Eigen::VectorXd torques; 
    Eigen::VectorXd initialPosture; 
    ocra_icub::OCRA_ICUB_MESSAGE controllerStatus;
    ControllerRpcServerCallback::shared_ptr rpcServerCallback; 
    yarp::os::RpcServer rpcServerPort; 
    // Debugging related
    int debugJointIndex;
    yarp::os::RpcServer debugRpcPort;
    yarp::os::Port debugRefOutPort;
    yarp::os::Port debugRealOutPort;
    DebugRpcServerCallback::shared_ptr debugRpcCallback; 
    Eigen::VectorXd measuredTorques;
    bool debuggingAllJoints;
    bool userHasSetDebugIndex;

    Eigen::Displacementd l_foot_disp_inverse; 
    iDynTree::SimpleLeggedOdometry odometry; 
};


#endif // OCRA_CONTROLLER_SERVER_THREAD_H