Statistics
| Branch: | Tag: | Revision:

humotion / src / server / server.cpp @ f10ec142

History | View | Annotate | Download (5.168 KB)

1
/*
2
* This file is part of humotion
3
*
4
* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
5
* http://opensource.cit-ec.de/projects/humotion
6
*
7
* This file may be licensed under the terms of the
8
* GNU Lesser General Public License Version 3 (the ``LGPL''),
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 LGPL for the specific language
14
* governing rights and limitations.
15
*
16
* You should have received a copy of the LGPL along with this
17
* program. If not, go to http://www.gnu.org/licenses/lgpl.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
#include "server/server.h"
29
#include "server/middleware_ros.h"
30
#include "server/middleware_rsb.h"
31
#include <boost/algorithm/string.hpp>
32
#include <string>
33

    
34
using namespace std;
35
using namespace boost;
36
using namespace humotion;
37
using namespace humotion::server;
38

    
39
//! set up constant for updaterate (50Hz)
40
const float Server::MOTION_UPDATERATE = 50.0;
41

    
42
//! constructor
43
//! open a new server instance.
44
Server::Server(string scope, string mw, JointInterface *_joint_interface){
45
    //convert mw to uppercase:
46
    to_upper(mw);
47

    
48
    printf("> initializing humotion server (on %s, middleware=%s)\n",scope.c_str(),mw.c_str());
49

    
50
    //store pointer to joint interface
51
    joint_interface = _joint_interface;
52

    
53
    //tell joint interface our framerate:
54
    joint_interface->set_framerate(MOTION_UPDATERATE);
55

    
56
    //create controller
57
    controller = new Controller(joint_interface);
58
    controller->init_motion_generators();
59

    
60
    //start middleware:
61
    if (mw == "ROS"){
62
        middleware = new MiddlewareROS(scope, controller);
63
    }else if (mw == "RSB"){
64
        middleware = new MiddlewareRSB(scope, controller);
65
    }else{
66
        printf("> ERROR: invalid mw '%s' given. Please use ROS or RSB\n\n",mw.c_str());
67
        exit(EXIT_FAILURE);
68
    }
69

    
70
    //start motion generation thread:
71
    start_motion_generation_thread();
72

    
73
}
74

    
75
//! destructor
76
Server::~Server(){
77
}
78

    
79
//! middleware still running?
80
bool Server::ok(){
81
    return middleware->ok();
82
}
83

    
84
//! start main thread
85
void Server::start_motion_generation_thread(){
86
    motion_generation_thread_ptr = new boost::thread(boost::bind( &Server::motion_generation_thread, this));
87
}
88

    
89
//! main thread that handles all data in/out
90
//! this thread will take care of gathering all input data and
91
//! then generate the output targets at a fixed rate of \sa MOTION_UPDATERATE
92
void Server::motion_generation_thread(){
93
    unsigned int incoming_data_count_invalid = 0;
94

    
95
    printf("> started motion generation thread\n");
96

    
97
    //calculate loop delay:
98
    float loop_delay = 1000.0 / MOTION_UPDATERATE;
99
    boost::system_time timeout = get_system_time() + posix_time::milliseconds(loop_delay);
100
    printf("> one loop = %3.1fms\n",loop_delay);
101

    
102
    //wait for incoming joint data:
103
    while (middleware->ok()){
104
        //mw tick
105
        middleware->tick();
106

    
107
        unsigned int incoming_data_count = joint_interface->get_and_clear_incoming_position_count();
108
        if (incoming_data_count == 0){
109
                incoming_data_count_invalid++;
110
                if (incoming_data_count_invalid >= MOTION_UPDATERATE){
111
                    printf("> waiting for valid incoming joint data (position+velocity)\n");
112
                    incoming_data_count_invalid = 0;
113
                }
114
        }else{
115
            //fine, joint data is arriving, exit waiting loop
116
            break;
117
        }
118

    
119
        thread::sleep(timeout);
120
        timeout = get_system_time() + posix_time::milliseconds(loop_delay);
121
    }
122

    
123
    printf("> joint data arrived, control loop active.\n");
124
    controller->set_activated();
125

    
126
    //fine, data is arriving, activate and run control loop:
127
    while (middleware->ok()){
128
        //mw tick
129
        middleware->tick();
130

    
131
        //do some logging:
132
        //controller->dump_angles();
133

    
134
        //calculate all targets
135
        controller->calculate_targets();
136

    
137
        //publish data to joints
138
        controller->publish_targets();
139

    
140
        //finally execute the movement:
141
        joint_interface->execute_motion();
142

    
143
        //check if we received joint information in the last step:
144
        unsigned int incoming_data_count = joint_interface->get_and_clear_incoming_position_count();
145
        if (incoming_data_count == 0){
146
                //printf("> WARNING: no incoming joint data during the last iteration...\n");
147
                incoming_data_count_invalid++;
148
                if (incoming_data_count_invalid >= MOTION_UPDATERATE){
149
                    printf("> ERROR: no incoming joint data for >1 second -> will exit now\n");
150
                    exit(EXIT_FAILURE);
151
                }
152
        }else{
153
                incoming_data_count_invalid = 0;
154
        }
155

    
156
        thread::sleep(timeout);
157
        timeout = get_system_time() + posix_time::milliseconds(loop_delay);
158
    }
159

    
160
    printf("> motion generation thread exited.\n");
161
}