Revision 0c8d22a5 examples/yarp_icub/src/icub_faceinterface.cpp
| examples/yarp_icub/src/icub_faceinterface.cpp | ||
|---|---|---|
| 28 | 28 |
#include "humotion_yarp_icub/icub_faceinterface.h" |
| 29 | 29 |
|
| 30 | 30 |
#include <boost/format.hpp> |
| 31 |
#include <boost/lexical_cast.hpp> |
|
| 32 |
|
|
| 33 | 31 |
#include <algorithm> |
| 34 | 32 |
#include <string> |
| 35 | 33 |
|
| ... | ... | |
| 48 | 46 |
cout << "opening connection to '"<< emotion_scope << "'\n"; |
| 49 | 47 |
|
| 50 | 48 |
bool init_ok = true; |
| 51 |
for (int i = 0; i < 4; i++) {
|
|
| 52 |
// strange, if we use one output port only the first command is executed?! flushing issues? |
|
| 53 |
std::string emotion_port_out = "/emotionwriter" + boost::lexical_cast<std::string>(i); |
|
| 54 |
if (!emotion_port[i].open(emotion_port_out.c_str())) {
|
|
| 55 |
cerr << "ERROR: failed to open emotion port '" << emotion_port_out << "'\n"; |
|
| 56 |
init_ok = false; |
|
| 57 |
} |
|
| 58 |
if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())) {
|
|
| 59 |
cerr << "ERROR: failed to connect to emotion port '" << emotion_port_out << "'\n"; |
|
| 60 |
init_ok = false; |
|
| 61 |
} |
|
| 49 |
// open emotion port |
|
| 50 |
std::string emotion_port_out = "/emotionwriter"; |
|
| 51 |
if (!emotion_port.open(emotion_port_out.c_str())) {
|
|
| 52 |
cerr << "ERROR: failed to open emotion port '" << emotion_port_out << "'\n"; |
|
| 53 |
init_ok = false; |
|
| 54 |
} |
|
| 55 |
if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())) {
|
|
| 56 |
cerr << "ERROR: failed to connect to emotion port '" << emotion_port_out << "'\n"; |
|
| 57 |
init_ok = false; |
|
| 62 | 58 |
} |
| 63 | 59 |
|
| 64 | 60 |
if (!init_ok) {
|
| ... | ... | |
| 76 | 72 |
//! special command to set eyelid angle |
| 77 | 73 |
//! \param angle in degrees |
| 78 | 74 |
void iCubFaceInterface::set_eyelid_angle(float angle) {
|
| 79 |
if (emotion_port[0].getOutputCount() > 0) {
|
|
| 75 |
if (emotion_port.getOutputCount() > 0) {
|
|
| 80 | 76 |
// try to set the value based on the upper one |
| 81 | 77 |
// some guesses from the sim: S30 = 0° / S40 = 10° |
| 82 | 78 |
int opening = (25.0 + 0.8*angle); |
| ... | ... | |
| 94 | 90 |
snprintf(buf, sizeof(buf), "S%2d", opening); |
| 95 | 91 |
|
| 96 | 92 |
// cout << "SETTING EYELID '" << buf << "' (" << angle << " -> " << opening << "\n";
|
| 97 |
Bottle &cmd = emotion_port[0].prepare();
|
|
| 93 |
Bottle &cmd = emotion_port.prepare(); |
|
| 98 | 94 |
cmd.clear(); |
| 99 | 95 |
cmd.addString(buf); |
| 100 |
emotion_port[0].writeStrict(); |
|
| 96 |
// NOTE: writeStrict is important in order not to loose packets |
|
| 97 |
emotion_port.writeStrict(); |
|
| 101 | 98 |
} else {
|
| 102 | 99 |
cerr << "ERROR: no icub emotion output\n"; |
| 103 | 100 |
exit(EXIT_FAILURE); |
| ... | ... | |
| 108 | 105 |
//! \param id {0=left, 1=right)
|
| 109 | 106 |
//! \param angle in degrees |
| 110 | 107 |
void iCubFaceInterface::set_eyebrow_angle(int id, float *target_angle) {
|
| 111 |
int port_id; |
|
| 112 |
if (id == iCubJointInterface::ICUB_ID_EYES_LEFT_BROW) {
|
|
| 113 |
port_id = 1; |
|
| 114 |
} else {
|
|
| 115 |
port_id = 2; |
|
| 116 |
} |
|
| 117 | 108 |
|
| 118 |
if (emotion_port[port_id].getOutputCount() > 0) {
|
|
| 109 |
if (emotion_port.getOutputCount() > 0) {
|
|
| 119 | 110 |
double angle = target_angle[id]; |
| 120 | 111 |
int icub_val = 0; |
| 121 | 112 |
|
| ... | ... | |
| 152 | 143 |
cmd_s = "R0" + boost::lexical_cast<std::string>(icub_val); |
| 153 | 144 |
} |
| 154 | 145 |
|
| 155 |
cout << "SETTING EYEBROW " << id << " (" << angle << " -> " << cmd_s << ")\n";
|
|
| 146 |
// cout << "SETTING EYEBROW " << id << " (" << angle << " -> " << cmd_s << ")\n";
|
|
| 156 | 147 |
|
| 157 |
Bottle &cmd = emotion_port[port_id].prepare();
|
|
| 148 |
Bottle &cmd = emotion_port.prepare(); |
|
| 158 | 149 |
cmd.clear(); |
| 159 | 150 |
cmd.addString(cmd_s); |
| 160 |
emotion_port[port_id].writeStrict(); |
|
| 151 |
// NOTE: writeStrict is important in order not to loose packets |
|
| 152 |
emotion_port.writeStrict(); |
|
| 161 | 153 |
} else {
|
| 162 | 154 |
cerr << "ERROR: no icub emotion output\n"; |
| 163 | 155 |
exit(EXIT_FAILURE); |
| ... | ... | |
| 244 | 236 |
*/ |
| 245 | 237 |
|
| 246 | 238 |
// add mouth |
| 247 |
Bottle &cmd = emotion_port[3].prepare();
|
|
| 239 |
Bottle &cmd = emotion_port.prepare(); |
|
| 248 | 240 |
cmd.clear(); |
| 249 | 241 |
cmd.addString(buf); |
| 250 |
emotion_port[3].writeStrict(); |
|
| 242 |
// NOTE: writeStrict is important in order not to loose packets |
|
| 243 |
emotion_port.writeStrict(); |
|
| 251 | 244 |
|
| 252 | 245 |
/* |
| 253 | 246 |
//store joint values which we do not handle on icub here: |
Also available in: Unified diff