humotion / src / server / middleware_rsb.cpp @ b1bbdb26
History | View | Annotate | Download (5.474 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/middleware_rsb.h" |
29 |
|
30 |
#ifdef RSB_SUPPORT
|
31 |
|
32 |
|
33 |
#define BOOST_SIGNALS_NO_DEPRECATION_WARNING 1 |
34 |
#include <rsb/converter/ProtocolBufferConverter.h> |
35 |
#include <rsb/converter/Repository.h> |
36 |
#include <rsb/Factory.h> |
37 |
#include <rsb/Listener.h> |
38 |
#include <rsb/MetaData.h> |
39 |
#include <rsb/patterns/RemoteServer.h> |
40 |
|
41 |
#include <string> |
42 |
|
43 |
// using namespace std;
|
44 |
// using namespace boost;
|
45 |
// using namespace humotion;
|
46 |
// using namespace humotion::server;
|
47 |
// using namespace rsb;
|
48 |
// using namespace rsb::patterns;
|
49 |
|
50 |
WARNING:
|
51 |
// RSB interface might be deprtecated and needs some backporting
|
52 |
// from the ROS code. [todo]
|
53 |
|
54 |
//! constructor
|
55 |
MiddlewareRSB::MiddlewareRSB(string scope, Controller *c) : Middleware(scope, c) {
|
56 |
printf("> using RSB middleware\n");
|
57 |
printf("> registering converters\n");
|
58 |
|
59 |
// converter for GazeTarget
|
60 |
rsb::converter::Converter<string>::Ptr gazeTargetConverter(
|
61 |
new rsb::converter::ProtocolBufferConverter<rst::robot::HumotionGazeTarget>());
|
62 |
rsb::converter::converterRepository<string>()->registerConverter(gazeTargetConverter);
|
63 |
|
64 |
// converter for MouthTarget
|
65 |
rsb::converter::Converter<string>::Ptr mouthTargetConverter(
|
66 |
new rsb::converter::ProtocolBufferConverter<rst::robot::MouthTarget>());
|
67 |
rsb::converter::converterRepository<string>()->registerConverter(mouthTargetConverter);
|
68 |
|
69 |
// first get a factory instance that is used to create RSB domain objects
|
70 |
Factory &factory = getFactory(); |
71 |
|
72 |
// create listeners
|
73 |
mouth_target_listener = factory.createListener(base_scope + "/humotion/mouth/target");
|
74 |
mouth_target_listener->addHandler(HandlerPtr( |
75 |
new DataFunctionHandler<rst::robot::MouthTarget>(
|
76 |
boost::bind(&MiddlewareRSB::incoming_mouth_target, this, _1))));
|
77 |
|
78 |
gaze_target_listener = factory.createListener(base_scope + "/humotion/gaze/target");
|
79 |
gaze_target_listener->addHandler(HandlerPtr( |
80 |
new EventFunctionHandler(
|
81 |
boost::bind(&MiddlewareRSB::incoming_gaze_target, this, _1))));
|
82 |
|
83 |
// informer->publish(gaze_target);
|
84 |
printf("> MiddlewareRSB initialised\n");
|
85 |
} |
86 |
|
87 |
|
88 |
//! destructor
|
89 |
MiddlewareRSB::~MiddlewareRSB() { |
90 |
} |
91 |
|
92 |
//! connection ok?
|
93 |
//! \return true if conn is alive
|
94 |
bool MiddlewareRSB::ok() {
|
95 |
return true; |
96 |
} |
97 |
|
98 |
//! do a single tick
|
99 |
void MiddlewareRSB::tick() {
|
100 |
// nothing to do
|
101 |
} |
102 |
|
103 |
//! callback to handle incoming mouth target
|
104 |
void MiddlewareRSB::incoming_mouth_target(boost::shared_ptr<rst::robot::MouthTarget> msg) {
|
105 |
// printf("> incoming mouth_target\n");
|
106 |
MouthState mouth_state; |
107 |
|
108 |
mouth_state.position_left = msg->position_left(); |
109 |
mouth_state.position_center = msg->position_center(); |
110 |
mouth_state.position_right = msg->position_right(); |
111 |
|
112 |
mouth_state.opening_left = msg->opening_left(); |
113 |
mouth_state.opening_center = msg->opening_center(); |
114 |
mouth_state.opening_right = msg->opening_right(); |
115 |
|
116 |
controller->set_mouth_target(mouth_state); |
117 |
} |
118 |
|
119 |
//! callback to handle incoming gaze target
|
120 |
void MiddlewareRSB::incoming_gaze_target(rsb::EventPtr event) {
|
121 |
// printf("> incoming gaze_target [P:%3.1f, T:%3.1f, R:%3.1f]\n",
|
122 |
// msg->pan(), msg->tilt(), msg->roll());
|
123 |
|
124 |
boost::shared_ptr<void> ev_data = event->getData();
|
125 |
rst::robot::HumotionGazeTarget *msg = (rst::robot::HumotionGazeTarget*) ev_data.get(); |
126 |
|
127 |
GazeState gaze_state; |
128 |
|
129 |
if (msg->gaze_type() == rst::robot::HumotionGazeTarget::ABSOLUTE) {
|
130 |
gaze_state.gaze_type = GazeState::ABSOLUTE; |
131 |
} else {
|
132 |
gaze_state.gaze_type = GazeState::RELATIVE; |
133 |
} |
134 |
|
135 |
gaze_state.pan = msg->pan(); |
136 |
gaze_state.tilt = msg->tilt(); |
137 |
gaze_state.roll = msg->roll(); |
138 |
gaze_state.vergence = msg->vergence(); |
139 |
|
140 |
gaze_state.pan_offset = msg->pan_offset(); |
141 |
gaze_state.tilt_offset = msg->tilt_offset(); |
142 |
gaze_state.roll_offset = msg->roll_offset(); |
143 |
|
144 |
gaze_state.eyelid_opening_upper = msg->eyelid_opening_upper(); |
145 |
gaze_state.eyelid_opening_lower = msg->eyelid_opening_lower(); |
146 |
|
147 |
gaze_state.eyebrow_left = msg->eyebrow_left(); |
148 |
gaze_state.eyebrow_right = msg->eyebrow_right(); |
149 |
|
150 |
gaze_state.eyeblink_request_left = msg->eyeblink_request_left(); |
151 |
gaze_state.eyeblink_request_right = msg->eyeblink_request_right(); |
152 |
|
153 |
gaze_state.timestamp = msg->gaze_timestamp(); |
154 |
// FIXME: convert RSB timestamp to our representation
|
155 |
// event->getMetaData().getCreateTime() / 1000.0;
|
156 |
// createTime() returns milliseconds -> convert to seconds
|
157 |
|
158 |
controller->set_gaze_target(gaze_state); |
159 |
} |
160 |
|
161 |
#endif
|