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