humotion / src / client / middleware_ros.cpp @ d3aa8ab3
History | View | Annotate | Download (4.107 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 "client/middleware_ros.h" |
29 |
|
30 |
#include "humotion/mouth.h" |
31 |
#include "humotion/gaze.h" |
32 |
#include <boost/algorithm/string/classification.hpp> |
33 |
|
34 |
using namespace std; |
35 |
using namespace boost; |
36 |
using namespace humotion; |
37 |
using namespace humotion::client; |
38 |
|
39 |
//! constructor
|
40 |
MiddlewareROS::MiddlewareROS(string scope) : Middleware(scope){
|
41 |
//start ros core
|
42 |
if (!ros::isInitialized()){
|
43 |
tick_necessary = true;
|
44 |
string node_name = "humotion_client__"+ base_scope; |
45 |
node_name.erase(std::remove(node_name.begin(), node_name.end(), '/'), node_name.end());
|
46 |
|
47 |
ros::M_string no_remapping; |
48 |
ros::init(no_remapping, node_name); |
49 |
}else{
|
50 |
//another ros thread takes care of spinning
|
51 |
tick_necessary = false;
|
52 |
} |
53 |
|
54 |
//create node handle
|
55 |
ros::NodeHandle n; |
56 |
|
57 |
//set up publishers:
|
58 |
mouth_target_publisher = n.advertise<humotion::mouth>(base_scope + "/humotion/mouth/target", 100); |
59 |
gaze_target_publisher = n.advertise<humotion::gaze>(base_scope + "/humotion/gaze/target", 100); |
60 |
|
61 |
} |
62 |
|
63 |
//! destructor
|
64 |
MiddlewareROS::~MiddlewareROS(){ |
65 |
} |
66 |
|
67 |
//! connection ok?
|
68 |
//! \return true if conn is alive
|
69 |
bool MiddlewareROS::ok(){
|
70 |
return ros::ok();
|
71 |
} |
72 |
|
73 |
//! do a single tick
|
74 |
void MiddlewareROS::tick(){
|
75 |
if (tick_necessary){
|
76 |
ros::spinOnce(); |
77 |
} |
78 |
} |
79 |
|
80 |
|
81 |
//! send mouth target to server
|
82 |
void MiddlewareROS::send_mouth_target(){
|
83 |
//build target packet:
|
84 |
humotion::mouth msg; |
85 |
|
86 |
//set timestamp
|
87 |
msg.header.stamp = ros::Time::now(); |
88 |
|
89 |
msg.position.left = mouth_state.position_left; |
90 |
msg.position.center = mouth_state.position_center; |
91 |
msg.position.right = mouth_state.position_right; |
92 |
|
93 |
msg.opening.left = mouth_state.opening_left; |
94 |
msg.opening.center = mouth_state.opening_center; |
95 |
msg.opening.right = mouth_state.opening_right; |
96 |
|
97 |
|
98 |
//add position to send queue
|
99 |
mouth_target_publisher.publish(msg); |
100 |
|
101 |
//allow ros to handle data
|
102 |
tick(); |
103 |
} |
104 |
|
105 |
//! send mouth target to server
|
106 |
void MiddlewareROS::send_gaze_target(){
|
107 |
//build target packet:
|
108 |
humotion::gaze msg; |
109 |
|
110 |
//set timestamp
|
111 |
msg.header.stamp = ros::Time::now(); |
112 |
|
113 |
msg.pan = gaze_state.pan; |
114 |
msg.tilt = gaze_state.tilt; |
115 |
msg.roll = gaze_state.roll; |
116 |
msg.vergence = gaze_state.vergence; |
117 |
|
118 |
msg.pan_offset = gaze_state.pan_offset; |
119 |
msg.tilt_offset = gaze_state.tilt_offset; |
120 |
msg.roll_offset = gaze_state.roll_offset; |
121 |
|
122 |
msg.eyelid_opening_upper = gaze_state.eyelid_opening_upper; |
123 |
msg.eyelid_opening_lower = gaze_state.eyelid_opening_lower; |
124 |
|
125 |
msg.eyebrow_left = gaze_state.eyebrow_left; |
126 |
msg.eyebrow_right = gaze_state.eyebrow_right; |
127 |
|
128 |
msg.eyeblink_request_left = gaze_state.eyeblink_request_left; |
129 |
msg.eyeblink_request_right = gaze_state.eyeblink_request_right; |
130 |
|
131 |
if (gaze_state.gaze_type == GazeState::GAZETYPE_ABSOLUTE){
|
132 |
msg.gaze_type = humotion::gaze::GAZETYPE_ABSOLUTE; |
133 |
}else{
|
134 |
msg.gaze_type = humotion::gaze::GAZETYPE_RELATIVE; |
135 |
} |
136 |
msg.gaze_timestamp.sec = gaze_state.timestamp.sec; |
137 |
msg.gaze_timestamp.nsec = gaze_state.timestamp.nsec; |
138 |
|
139 |
//add position to send queue
|
140 |
gaze_target_publisher.publish(msg); |
141 |
|
142 |
//allow ros to handle data
|
143 |
tick(); |
144 |
} |
145 |
|