Statistics
| Branch: | Tag: | Revision:

humotion / src / client / middleware_ros.cpp @ c119704b

History | View | Annotate | Download (4.33 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 <boost/algorithm/string/classification.hpp>
29
#include <string>
30

    
31
#include "humotion/client/middleware_ros.h"
32
#include "humotion/gaze.h"
33
#include "humotion/mouth.h"
34

    
35

    
36
// using namespace std;
37
// using namespace boost;
38
// using namespace humotion;
39
using humotion::client::MiddlewareROS;
40

    
41
//! constructor
42
MiddlewareROS::MiddlewareROS(std::string scope) : Middleware(scope) {
43
    // start ros core
44
    if (!ros::isInitialized()) {
45
        tick_necessary_ = true;
46
        std::string node_name = "humotion_client__"+ base_scope_;
47
        node_name.erase(std::remove(node_name.begin(), node_name.end(), '/'), node_name.end());
48

    
49
        ros::M_string no_remapping;
50
        ros::init(no_remapping, node_name);
51
    } else {
52
        // another ros thread already takes care of spinning
53
        tick_necessary_ = false;
54
    }
55

    
56
    // create node handle
57
    ros::NodeHandle n;
58

    
59
    // set up publishers
60
    mouth_target_publisher_ = n.advertise<humotion::mouth>(base_scope_ +
61
                                                          "/humotion/mouth/target", 100);
62
    gaze_target_publisher_  = n.advertise<humotion::gaze>(base_scope_ +
63
                                                         "/humotion/gaze/target", 100);
64
}
65

    
66
//! destructor
67
MiddlewareROS::~MiddlewareROS() {
68
}
69

    
70
//! connection ok?
71
//! \return true if conn is alive
72
bool MiddlewareROS::ok() {
73
    return ros::ok();
74
}
75

    
76
//! do a single tick
77
void MiddlewareROS::tick() {
78
    if (tick_necessary_) {
79
        ros::spinOnce();
80
    }
81
}
82

    
83

    
84
//! send mouth target to server
85
void MiddlewareROS::send_mouth_target() {
86
    // build target packet
87
    humotion::mouth msg;
88

    
89
    // set timestamp
90
    msg.header.stamp = ros::Time::now();
91

    
92
    msg.position.left   = mouth_state_.position_left;
93
    msg.position.center = mouth_state_.position_center;
94
    msg.position.right  = mouth_state_.position_right;
95

    
96
    msg.opening.left   = mouth_state_.opening_left;
97
    msg.opening.center = mouth_state_.opening_center;
98
    msg.opening.right  = mouth_state_.opening_right;
99

    
100

    
101
    // add position to send queue
102
    mouth_target_publisher_.publish(msg);
103

    
104
    // allow ros to handle data
105
    tick();
106
}
107

    
108
//! send mouth target to server
109
void MiddlewareROS::send_gaze_target() {
110
    // build target packet
111
    humotion::gaze msg;
112

    
113
    // set timestamp
114
    msg.header.stamp = ros::Time::now();
115

    
116
    msg.pan   = gaze_state_.pan;
117
    msg.tilt  = gaze_state_.tilt;
118
    msg.roll  = gaze_state_.roll;
119
    msg.vergence = gaze_state_.vergence;
120

    
121
    msg.pan_offset = gaze_state_.pan_offset;
122
    msg.tilt_offset = gaze_state_.tilt_offset;
123
    msg.roll_offset = gaze_state_.roll_offset;
124

    
125
    msg.eyelid_opening_upper = gaze_state_.eyelid_opening_upper;
126
    msg.eyelid_opening_lower = gaze_state_.eyelid_opening_lower;
127

    
128
    msg.eyebrow_left = gaze_state_.eyebrow_left;
129
    msg.eyebrow_right = gaze_state_.eyebrow_right;
130

    
131
    msg.eyeblink_request_left = gaze_state_.eyeblink_request_left;
132
    msg.eyeblink_request_right = gaze_state_.eyeblink_request_right;
133

    
134
    if (gaze_state_.gaze_type == GazeState::GAZETYPE_ABSOLUTE) {
135
        msg.gaze_type = humotion::gaze::GAZETYPE_ABSOLUTE;
136
    } else {
137
        msg.gaze_type = humotion::gaze::GAZETYPE_RELATIVE;
138
    }
139
    msg.gaze_timestamp.sec = gaze_state_.timestamp.sec;
140
    msg.gaze_timestamp.nsec = gaze_state_.timestamp.nsec;
141

    
142
    // add position to send queue
143
    gaze_target_publisher_.publish(msg);
144

    
145
    // allow ros to handle data
146
    tick();
147
}
148