humotion

#include <stdio.h>

#include <yarp/os/Network.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include <yarp/dev/PolyDriver.h>

#include <yarp/os/Time.h>

#include <yarp/sig/Vector.h>

#include "icub_jointinterface.h"

#include <humotion/server/server.h>

#include <yarp/os/Network.h>

#include <yarp/os/RateThread.h>

#include <yarp/os/Time.h>

#include <yarp/os/Property.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include <string>

#include <iostream>

#include <icub_data_receiver.h>

using namespace yarp::dev;

using namespace yarp::sig;

using namespace yarp::os;

using namespace std;

int main(int argc, char *argv[]){

    Network yarp;

    if (!yarp.checkNetwork()){

        printf("No yarp network, quitting\n");

        return false;

    }

    Property params;

    params.fromCommand(argc, argv);

    if (!params.check("robot")){

        fprintf(stderr, "Please specify the name of the robot\n");

        fprintf(stderr, "--robot name (e.g. icub or icubSim)\n");

        return -1;

    }

    string robotName=params.find("robot").asString().c_str();

    string scope="/"+robotName;

    //create humotion interface

    iCubJointInterface *icub_jointinterface = new iCubJointInterface(scope);

    humotion::server::Server *humotion_server = new humotion::server::Server("/icub", "ROS", icub_jointinterface);

    icub_jointinterface->run();

    while(humotion_server->ok()){

        usleep(100000);

    }

    return 0;

}