hlrc

/*

* This file is part of hlrc_server

*

* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de

* http://opensource.cit-ec.de/projects/hlrc_server

*

* This file may be licensed under the terms of the

* GNU General Public License Version 3 (the ``GPL''),

* or (at your option) any later version.

*

* Software distributed under the License is distributed

* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either

* express or implied. See the GPL for the specific language

* governing rights and limitations.

*

* You should have received a copy of the GPL along with this

* program. If not, go to http://www.gnu.org/licenses/gpl.html

* or write to the Free Software Foundation, Inc.,

* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

*

* The development of this software was supported by the

* Excellence Cluster EXC 277 Cognitive Interaction Technology.

* The Excellence Cluster EXC 277 is a grant of the Deutsche

* Forschungsgemeinschaft (DFG) in the context of the German

* Excellence Initiative.

*

*/

#include "MiddlewareRSB.h"

#include "MiddlewareROS.h"

#include "Arbiter.h"

#include <string>

#include <iostream>

#include <stdlib.h>

#include <math.h>

#include <chrono>

#include <thread>

#include <boost/algorithm/string.hpp>

Arbiter *arbiter;

using namespace std;

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

    if ((argc != 5) && (argc != 6)){

        printf("> ERROR: invalid number of parameters passed to server!\n\nusage: %s <mw_hlrc> <mw_humotion> <base scopename hlrc> <base scopename humotion> [<audio output>]  (example: %s RSB RSB /flobi1/ pulse )\n",argv[0],argv[0]);

        printf("          audio output is optional and can be {pulse,oss,alsa,null,sndio,..}\n\n");

        exit(EXIT_FAILURE);

    }

    printf("> hlrc_server starting\n");

    //fetch scope from cmd line

    std::string scope_hlrc = argv[3];

    std::string scope_humotion = argv[4];

    //fetch middleware from cmd line

    std::string mw = argv[1];

    boost::algorithm::to_upper(mw); //convert to uppercase

    //fetch mw for humotion

    std::string mw_humotion = argv[2];

    //arbiter

    std::string sound_output = "pulse";

    if (argc == 6){

        sound_output = argv[5];

    }

    arbiter = new Arbiter(sound_output);

    //mw connection

    Middleware *middleware;

    if (mw == "ROS"){

        middleware = new MiddlewareROS(arbiter, scope_hlrc);

    }else{

        middleware = new MiddlewareRSB(arbiter, scope_hlrc);

    }

    //human motion connection:

    humotion::client::Client *client = new humotion::client::Client(scope_humotion, mw_humotion);

    printf("> all done. hlrc server ready\n");

    //send values to human motion server with 50Hz

    const auto loop_period = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::duration<float>(1.0 / 50));

    auto timeout = std::chrono::steady_clock::now() + loop_period;

    while(client->ok()){

        middleware->tick();

        //find correct arbitration

        arbiter->arbitrate();

        //periodically send mouth & gaze target

        client->update_mouth_target(arbiter->get_mouth_state());

        client->update_gaze_target(arbiter->get_gaze_state());

        client->send_all();

        //debug:

        //arbiter->get_mouth_state().dump();

        std::this_thread::sleep_until(timeout);

        timeout += loop_period;

    }

    return EXIT_SUCCESS;

}