Statistics
| Branch: | Tag: | Revision:

hlrc / server / src / main.cpp @ d8feb404

History | View | Annotate | Download (3.114 KB)

1 0c286af0 Simon Schulz
/*
2 f150aab5 Robert Haschke
 * This file is part of hlrc_server
3
 *
4
 * Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
5
 * http://opensource.cit-ec.de/projects/hlrc_server
6
 *
7
 * This file may be licensed under the terms of the
8
 * GNU General Public License Version 3 (the ``GPL''),
9
 * or (at your option) any later version.
10
 *
11
 * Software distributed under the License is distributed
12
 * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
13
 * express or implied. See the GPL for the specific language
14
 * governing rights and limitations.
15
 *
16
 * You should have received a copy of the GPL along with this
17
 * program. If not, go to http://www.gnu.org/licenses/gpl.html
18
 * or write to the Free Software Foundation, Inc.,
19
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20
 *
21
 * The development of this software was supported by the
22
 * Excellence Cluster EXC 277 Cognitive Interaction Technology.
23
 * The Excellence Cluster EXC 277 is a grant of the Deutsche
24
 * Forschungsgemeinschaft (DFG) in the context of the German
25
 * Excellence Initiative.
26
 *
27
 */
28 0c286af0 Simon Schulz
29
#include "MiddlewareROS.h"
30
#include "Arbiter.h"
31
32
#include <string>
33
#include <iostream>
34
#include <stdlib.h>
35
#include <math.h>
36 90fa4b4e Robert Haschke
#include <chrono>
37
#include <thread>
38 0c286af0 Simon Schulz
#include <boost/algorithm/string.hpp>
39
40 f150aab5 Robert Haschke
Arbiter* arbiter;
41 0c286af0 Simon Schulz
42
using namespace std;
43
44 f150aab5 Robert Haschke
int main(int argc, char* argv[]) {
45
        if ((argc != 5) && (argc != 6)) {
46
                printf("> ERROR: invalid number of parameters passed to server!\n\nusage: %s <mw_hlrc> <mw_humotion> <base scopename "
47 61ca8481 robocamp
                       "hlrc> <base scopename humotion> [<audio output>]  \n",
48 f150aab5 Robert Haschke
                       argv[0], argv[0]);
49
                printf("          audio output is optional and can be {pulse,oss,alsa,null,sndio,..}\n\n");
50
                exit(EXIT_FAILURE);
51
        }
52 0c286af0 Simon Schulz
53 f150aab5 Robert Haschke
        printf("> hlrc_server starting\n");
54 0c286af0 Simon Schulz
55 f150aab5 Robert Haschke
        // fetch scope from cmd line
56
        std::string scope_hlrc = argv[3];
57
        std::string scope_humotion = argv[4];
58 0c286af0 Simon Schulz
59 f150aab5 Robert Haschke
        // fetch middleware from cmd line
60
        std::string mw = argv[1];
61 90fa4b4e Robert Haschke
        boost::algorithm::to_upper(mw); // convert to uppercase
62 0c286af0 Simon Schulz
63 f150aab5 Robert Haschke
        // fetch mw for humotion
64
        std::string mw_humotion = argv[2];
65 4b075581 Simon Schulz
66 f150aab5 Robert Haschke
        // arbiter
67
        std::string sound_output = "pulse";
68
        if (argc == 6) {
69
                sound_output = argv[5];
70
        }
71
        arbiter = new Arbiter(sound_output);
72 0c286af0 Simon Schulz
73 f150aab5 Robert Haschke
        // mw connection
74
        Middleware* middleware;
75
        if (mw == "ROS") {
76
                middleware = new MiddlewareROS(arbiter, scope_hlrc);
77
        }
78
        else {
79 61ca8481 robocamp
      printf("> ERROR: invalid middleware! Only ROS supported)\n");
80 f150aab5 Robert Haschke
        }
81 0c286af0 Simon Schulz
82 f150aab5 Robert Haschke
        // human motion connection:
83
        humotion::client::Client* client = new humotion::client::Client(scope_humotion, mw_humotion);
84 0c286af0 Simon Schulz
85 f150aab5 Robert Haschke
        printf("> all done. hlrc server ready\n");
86 0c286af0 Simon Schulz
87 90fa4b4e Robert Haschke
        // send values to human motion server with 50Hz
88
        const auto loop_period = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::duration<float>(1.0 / 50));
89
        auto timeout = std::chrono::steady_clock::now() + loop_period;
90 0c286af0 Simon Schulz
91 f150aab5 Robert Haschke
        while (client->ok()) {
92
                middleware->tick();
93 0c286af0 Simon Schulz
94 f150aab5 Robert Haschke
                // find correct arbitration
95
                arbiter->arbitrate();
96 0c286af0 Simon Schulz
97 f150aab5 Robert Haschke
                // periodically send mouth & gaze target
98
                client->update_mouth_target(arbiter->get_mouth_state());
99
                client->update_gaze_target(arbiter->get_gaze_state());
100
                client->send_all();
101 0c286af0 Simon Schulz
102 f150aab5 Robert Haschke
                // debug:
103
                // arbiter->get_mouth_state().dump();
104 0c286af0 Simon Schulz
105 90fa4b4e Robert Haschke
                std::this_thread::sleep_until(timeout);
106
                timeout += loop_period;
107 f150aab5 Robert Haschke
        }
108 0c286af0 Simon Schulz
109 f150aab5 Robert Haschke
        return EXIT_SUCCESS;
110 0c286af0 Simon Schulz
}