subSystem_KARMA.h

Go to the documentation of this file.

/*
* Copyright (C) 2014 WYSIWYD Consortium, European Commission FP7 Project ICT-612139
* Authors: NGUYEN Dong Hai Phuong
* email:   phuong.nguyen@iit.it
* website: http://wysiwyd.upf.edu/
* Permission is granted to copy, distribute, and/or modify this program
* under the terms of the GNU General Public License, version 2 or any
* later version published by the Free Software Foundation.
*
* A copy of the license can be found at
* icub-client/license/gpl.txt
*
* 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
*/

#ifndef SUBSYSTEM_KARMA_H
#define SUBSYSTEM_KARMA_H

#include <string>
#include <algorithm>
#include <yarp/os/all.h>
#include <yarp/sig/all.h>
#include <yarp/dev/all.h>

#include "icubclient/subsystems/subSystem.h"
#include "icubclient/subsystems/subSystem_ARE.h"

#define SUBSYSTEM_KARMA       "KARMA"

namespace icubclient {
class SubSystem_KARMA : public SubSystem
{
protected:
    SubSystem_ARE* SubARE;
    bool AREconnected;

    std::string robot;
    double tableHeight;
    bool hasTable;

    yarp::os::RpcClient portStop;
    yarp::os::RpcClient portVision;
    yarp::os::RpcClient portFinder;
    yarp::os::RpcClient portCalib;

    //testing Cartesian interface
    yarp::dev::PolyDriver driverL;
    yarp::dev::PolyDriver driverR;
    yarp::dev::PolyDriver driverHL;
    yarp::dev::PolyDriver driverHR;

    yarp::dev::ICartesianControl *iCartCtrlL;
    yarp::dev::ICartesianControl *iCartCtrlR;

    void appendTarget(yarp::os::Bottle& b, const yarp::sig::VectorOf<double> &tCenter);

    void appendDouble(yarp::os::Bottle& b, const double &v);

    bool returnArmSafely(std::string armType);

    void selectHandCorrectTarget(yarp::os::Bottle& options, const std::string &targetName,
                                 yarp::sig::VectorOf<double>& target,
                                 const std::string handToUse="");

    bool sendCmd(yarp::os::Bottle &cmd);

    bool connect();

public:
    SubSystem_KARMA(const std::string &masterName, const std::string &robot);

    void Close();

    yarp::sig::VectorOf<double> applySafetyMargins(const yarp::sig::VectorOf<double>& in);

    bool chooseArm(const std::string &armType);

    void chooseArmAuto();

    bool pushAside(const std::string &objName,
                   const yarp::sig::VectorOf<double> &objCenter, const double &targetPosY,
                   const double &theta,
                   const std::string &armType = "selectable",
                   const yarp::os::Bottle &options = yarp::os::Bottle());

    bool pushFront(const std::string &objName,
                   const yarp::sig::VectorOf<double> &objCenter, const double &targetPosXFront,
                   const std::string &armType = "selectable",
                   const yarp::os::Bottle &options = yarp::os::Bottle());

    bool push(const yarp::sig::VectorOf<double> &targetCenter, const double theta, const double radius,
              const yarp::os::Bottle &options = yarp::os::Bottle());

    bool pullBack(const std::string &objName,
                  const yarp::sig::VectorOf<double> &objCenter, const double &targetPosXBack,
                  const std::string &armType = "selectable",
                  const yarp::os::Bottle &options = yarp::os::Bottle());
    bool draw(const yarp::sig::VectorOf<double> &targetCenter, const double theta,
              const double radius, const double dist,
              const yarp::os::Bottle &options = yarp::os::Bottle());

    bool vdraw(const std::string &objName,
               const yarp::sig::VectorOf<double> &targetCenter, const double theta,
               const double radius, const double dist,
               const yarp::os::Bottle &options = yarp::os::Bottle());

    bool openCartesianClient();

    ~SubSystem_KARMA();
};
}

#endif // SUBSYSTEM_KARMA_H