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 "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\n"

                "usage: %s <mw_hlrc> <mw_humotion> <base scopename hlrc> <base scopename humotion> [<audio output>]\n"

                "(example: %s ROS ROS /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 {

      printf("> ERROR: invalid middleware! Only ROS supported)\n");

        }

        // 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;

}