| 32 |
32 |
#include <rsb/converter/Repository.h>
|
| 33 |
33 |
#include <rsb/converter/ProtocolBufferConverter.h>
|
| 34 |
34 |
|
| 35 |
|
#include <rst/robot/EmotionState.pb.h>
|
| 36 |
|
#include <rst/robot/Animation.pb.h>
|
| 37 |
|
#include <rst/robot/GazeTarget.pb.h>
|
| 38 |
|
#include <rst/robot/MouthTarget.pb.h>
|
|
35 |
#include <rst/animation/EmotionExpression.pb.h>
|
|
36 |
#include <rst/animation/BinocularHeadGaze.pb.h>
|
|
37 |
#include <rst/animation/HeadAnimation.pb.h>
|
| 39 |
38 |
#include <rst/audition/Utterance.pb.h>
|
|
39 |
#include <rst/audition/SoundChunk.pb.h>
|
| 40 |
40 |
#include <boost/algorithm/string.hpp>
|
| 41 |
41 |
#include <boost/range/algorithm_ext/erase.hpp>
|
| 42 |
42 |
|
| 43 |
|
WARNING: RSB interface might be deprtecated and needs some backporting
|
| 44 |
|
from the ROS code. [todo]
|
| 45 |
|
|
| 46 |
43 |
using namespace std;
|
| 47 |
44 |
using namespace rsb;
|
| 48 |
45 |
using namespace rsb::patterns;
|
| ... | ... | |
| 57 |
54 |
|
| 58 |
55 |
try{
|
| 59 |
56 |
//converter for EmotionState
|
| 60 |
|
rsb::converter::Converter<string>::Ptr emotionStateConverter(new rsb::converter::ProtocolBufferConverter<rst::robot::EmotionState>());
|
|
57 |
rsb::converter::Converter<string>::Ptr emotionStateConverter(new rsb::converter::ProtocolBufferConverter<rst::animation::EmotionExpression>());
|
| 61 |
58 |
rsb::converter::converterRepository<string>()->registerConverter(emotionStateConverter);
|
| 62 |
59 |
|
| 63 |
60 |
//converter for Utterance
|
| ... | ... | |
| 65 |
62 |
//rsb::converter::converterRepository<string>()->registerConverter(UtteranceConverter);
|
| 66 |
63 |
|
| 67 |
64 |
//converter for GazeTarget
|
| 68 |
|
rsb::converter::Converter<string>::Ptr gazeTargetConverter(new rsb::converter::ProtocolBufferConverter<rst::robot::GazeTarget>());
|
|
65 |
rsb::converter::Converter<string>::Ptr gazeTargetConverter(new rsb::converter::ProtocolBufferConverter<rst::animation::BinocularHeadGaze>());
|
| 69 |
66 |
rsb::converter::converterRepository<string>()->registerConverter(gazeTargetConverter);
|
| 70 |
67 |
|
| 71 |
68 |
//converter for MouthTarget
|
| 72 |
|
rsb::converter::Converter<string>::Ptr mouthTargetConverter(new rsb::converter::ProtocolBufferConverter<rst::robot::MouthTarget>());
|
| 73 |
|
rsb::converter::converterRepository<string>()->registerConverter(mouthTargetConverter);
|
|
69 |
///rsb::converter::Converter<string>::Ptr mouthTargetConverter(new rsb::converter::ProtocolBufferConverter<rst::robot::MouthTarget>());
|
|
70 |
///rsb::converter::converterRepository<string>()->registerConverter(mouthTargetConverter);
|
| 74 |
71 |
|
| 75 |
72 |
|
| 76 |
73 |
//converter for Animation
|
| 77 |
|
rsb::converter::Converter<string>::Ptr animationConverter(new rsb::converter::ProtocolBufferConverter<rst::robot::Animation>());
|
|
74 |
rsb::converter::Converter<string>::Ptr animationConverter(new rsb::converter::ProtocolBufferConverter<rst::animation::HeadAnimation>());
|
| 78 |
75 |
rsb::converter::converterRepository<string>()->registerConverter(animationConverter);
|
| 79 |
76 |
|
| 80 |
77 |
}catch(std::invalid_argument e){
|
| ... | ... | |
| 93 |
90 |
}
|
| 94 |
91 |
|
| 95 |
92 |
void MiddlewareRSB::publish_emotion(string scope_target, RobotEmotion e, bool blocking){
|
| 96 |
|
boost::shared_ptr<rst::robot::EmotionState> request(new rst::robot::EmotionState());
|
|
93 |
boost::shared_ptr<rst::animation::EmotionExpression> request(new rst::animation::EmotionExpression());
|
| 97 |
94 |
|
| 98 |
95 |
switch(e.value){
|
| 99 |
96 |
default:
|
| 100 |
97 |
printf("> WANRING: invalid emotion id %d. defaulting to NEUTRAL\n",e.value);
|
| 101 |
98 |
//fall through:
|
| 102 |
99 |
case(RobotEmotion::NEUTRAL):
|
| 103 |
|
request->set_value(rst::robot::EmotionState::NEUTRAL);
|
|
100 |
request->set_emotion(rst::animation::EmotionExpression::NEUTRAL);
|
| 104 |
101 |
break;
|
| 105 |
102 |
case(RobotEmotion::HAPPY):
|
| 106 |
|
request->set_value(rst::robot::EmotionState::HAPPY);
|
|
103 |
request->set_emotion(rst::animation::EmotionExpression::HAPPY);
|
| 107 |
104 |
break;
|
| 108 |
105 |
case(RobotEmotion::SAD):
|
| 109 |
|
request->set_value(rst::robot::EmotionState::SAD);
|
|
106 |
request->set_emotion(rst::animation::EmotionExpression::SAD);
|
| 110 |
107 |
break;
|
| 111 |
108 |
case(RobotEmotion::ANGRY):
|
| 112 |
|
request->set_value(rst::robot::EmotionState::ANGRY);
|
|
109 |
request->set_emotion(rst::animation::EmotionExpression::ANGRY);
|
| 113 |
110 |
break;
|
| 114 |
111 |
case(RobotEmotion::SURPRISED):
|
| 115 |
|
request->set_value(rst::robot::EmotionState::SURPRISED);
|
|
112 |
request->set_emotion(rst::animation::EmotionExpression::SURPRISED);
|
| 116 |
113 |
break;
|
| 117 |
114 |
case(RobotEmotion::FEAR):
|
| 118 |
|
request->set_value(rst::robot::EmotionState::FEAR);
|
|
115 |
request->set_emotion(rst::animation::EmotionExpression::FEAR);
|
| 119 |
116 |
break;
|
| 120 |
117 |
}
|
| 121 |
118 |
|
| 122 |
119 |
request->set_duration(e.time_ms);
|
| 123 |
120 |
|
| 124 |
121 |
if (blocking){
|
| 125 |
|
hlrc_server->call<rst::robot::EmotionState>(scope_target, request);
|
|
122 |
hlrc_server->call<rst::animation::EmotionExpression>(scope_target, request);
|
| 126 |
123 |
}else{
|
| 127 |
|
hlrc_server->callAsync<rst::robot::EmotionState>(scope_target, request);
|
|
124 |
hlrc_server->callAsync<rst::animation::EmotionExpression>(scope_target, request);
|
| 128 |
125 |
}
|
| 129 |
126 |
}
|
| 130 |
127 |
|
| ... | ... | |
| 137 |
134 |
publish_emotion("defaultEmotion", e, blocking);
|
| 138 |
135 |
}
|
| 139 |
136 |
|
| 140 |
|
void MiddlewareRSB::publish_gaze_target(RobotGaze target, bool blocking){
|
| 141 |
|
boost::shared_ptr<rst::robot::GazeTarget> request(new rst::robot::GazeTarget());
|
|
137 |
void MiddlewareRSB::publish_gaze_target(RobotGaze incoming_target, bool blocking){
|
|
138 |
boost::shared_ptr<rst::animation::BinocularHeadGaze> request(new rst::animation::BinocularHeadGaze ());
|
|
139 |
|
|
140 |
boost::shared_ptr<rst::geometry::SphericalDirectionFloat> target(new rst::geometry::SphericalDirectionFloat ());
|
|
141 |
target->set_azimuth(incoming_target.pan);
|
|
142 |
target->set_elevation(incoming_target.tilt);
|
|
143 |
request->set_allocated_target(target.get());
|
| 142 |
144 |
|
| 143 |
|
request->set_pan(target.pan);
|
| 144 |
|
request->set_tilt(target.tilt);
|
| 145 |
|
request->set_roll(target.roll);
|
| 146 |
|
request->set_vergence(target.vergence);
|
| 147 |
|
request->set_pan_offset(target.pan_offset);
|
| 148 |
|
request->set_tilt_offset(target.tilt_offset);
|
|
145 |
request->set_eye_vergence(incoming_target.vergence);
|
|
146 |
|
|
147 |
boost::shared_ptr<rst::geometry::SphericalDirectionFloat> offset(new rst::geometry::SphericalDirectionFloat());
|
|
148 |
offset->set_azimuth(incoming_target.pan_offset);
|
|
149 |
offset->set_elevation(incoming_target.tilt_offset);
|
|
150 |
request->set_allocated_offset(offset.get());
|
| 149 |
151 |
|
| 150 |
152 |
if (blocking){
|
| 151 |
|
hlrc_server->call<rst::robot::GazeTarget>("gaze", request);
|
|
153 |
hlrc_server->call<rst::animation::BinocularHeadGaze >("gaze", request);
|
| 152 |
154 |
}else{
|
| 153 |
|
hlrc_server->callAsync<rst::robot::GazeTarget>("gaze", request);
|
|
155 |
hlrc_server->callAsync<rst::animation::BinocularHeadGaze >("gaze", request);
|
| 154 |
156 |
}
|
| 155 |
157 |
}
|
| 156 |
158 |
|
| 157 |
159 |
void MiddlewareRSB::publish_mouth_target(RobotMouth target, bool blocking){
|
|
160 |
/*
|
| 158 |
161 |
boost::shared_ptr<rst::robot::MouthTarget> request(new rst::robot::MouthTarget());
|
| 159 |
162 |
|
| 160 |
163 |
request->set_position_left( target.position_left);
|
| ... | ... | |
| 170 |
173 |
}else{
|
| 171 |
174 |
hlrc_server->callAsync<rst::robot::MouthTarget>("mouth", request);
|
| 172 |
175 |
}
|
|
176 |
*/
|
|
177 |
printf("> ERROR: mouth targets not yet implemented in RSB middleware!\n");
|
| 173 |
178 |
}
|
| 174 |
179 |
|
| 175 |
180 |
void MiddlewareRSB::publish_head_animation(RobotHeadAnimation a, bool blocking){
|
| 176 |
|
boost::shared_ptr<rst::robot::Animation> request(new rst::robot::Animation());
|
|
181 |
boost::shared_ptr<rst::animation::HeadAnimation> request(new rst::animation::HeadAnimation());
|
| 177 |
182 |
|
| 178 |
183 |
switch(a.value){
|
| 179 |
184 |
default:
|
| 180 |
185 |
printf("> WANRING: invalid animation id %d. defaulting to IDLE",a.value);
|
| 181 |
186 |
//fall through:
|
| 182 |
187 |
case(RobotHeadAnimation::IDLE):
|
| 183 |
|
request->set_target(rst::robot::Animation::IDLE);
|
|
188 |
request->set_animation(rst::animation::HeadAnimation::IDLE);
|
| 184 |
189 |
break;
|
| 185 |
190 |
case(RobotHeadAnimation::HEAD_NOD):
|
| 186 |
|
request->set_target(rst::robot::Animation::HEAD_NOD);
|
|
191 |
request->set_animation(rst::animation::HeadAnimation::HEAD_NOD);
|
| 187 |
192 |
break;
|
| 188 |
193 |
case(RobotHeadAnimation::HEAD_SHAKE):
|
| 189 |
|
request->set_target(rst::robot::Animation::HEAD_SHAKE);
|
|
194 |
request->set_animation(rst::animation::HeadAnimation::HEAD_SHAKE);
|
| 190 |
195 |
break;
|
| 191 |
196 |
case(RobotHeadAnimation::EYEBLINK_L):
|
| 192 |
|
request->set_target(rst::robot::Animation::EYEBLINK_L);
|
|
197 |
request->set_animation(rst::animation::HeadAnimation::EYEBLINK_LEFT);
|
| 193 |
198 |
break;
|
| 194 |
199 |
case(RobotHeadAnimation::EYEBLINK_R):
|
| 195 |
|
request->set_target(rst::robot::Animation::EYEBLINK_R);
|
|
200 |
request->set_animation(rst::animation::HeadAnimation::EYEBLINK_RIGHT);
|
| 196 |
201 |
break;
|
| 197 |
202 |
case(RobotHeadAnimation::EYEBLINK_BOTH):
|
| 198 |
|
request->set_target(rst::robot::Animation::EYEBLINK_BOTH);
|
|
203 |
request->set_animation(rst::animation::HeadAnimation::EYEBLINK_BOTH);
|
| 199 |
204 |
break;
|
| 200 |
205 |
case(RobotHeadAnimation::EYEBROWS_RAISE):
|
| 201 |
|
request->set_target(rst::robot::Animation::EYEBROWS_RAISE);
|
|
206 |
request->set_animation(rst::animation::HeadAnimation::EYEBROWS_RAISE);
|
| 202 |
207 |
break;
|
| 203 |
208 |
case(RobotHeadAnimation::EYEBROWS_LOWER):
|
| 204 |
|
request->set_target(rst::robot::Animation::EYEBROWS_LOWER);
|
|
209 |
request->set_animation(rst::animation::HeadAnimation::EYEBROWS_LOWER);
|
|
210 |
break;
|
|
211 |
case(RobotHeadAnimation::ENGAGEMENT_LEFT):
|
|
212 |
request->set_animation(rst::animation::HeadAnimation::ENGAGEMENT_LEFT);
|
|
213 |
break;
|
|
214 |
case(RobotHeadAnimation::ENGAGEMENT_RIGHT):
|
|
215 |
request->set_animation(rst::animation::HeadAnimation::ENGAGEMENT_RIGHT);
|
| 205 |
216 |
break;
|
| 206 |
217 |
}
|
| 207 |
218 |
|
| 208 |
219 |
request->set_repetitions(a.repetitions);
|
| 209 |
|
request->set_scale(a.scale);
|
|
220 |
request->set_emphasis_scale(a.scale);
|
| 210 |
221 |
request->set_duration_each(a.time_ms);
|
| 211 |
222 |
|
| 212 |
223 |
if (blocking){
|
| 213 |
|
hlrc_server->call<rst::robot::Animation>("animation", request);
|
|
224 |
hlrc_server->call<rst::animation::HeadAnimation>("animation", request);
|
| 214 |
225 |
}else{
|
| 215 |
|
hlrc_server->callAsync<rst::robot::Animation>("animation", request);
|
|
226 |
hlrc_server->callAsync<rst::animation::HeadAnimation>("animation", request);
|
| 216 |
227 |
}
|
| 217 |
228 |
}
|
| 218 |
229 |
|