23 #include "led_thread.h"
25 #include "dcm_utils.h"
27 #include <alcore/alerror.h>
28 #include <almemoryfastaccess/almemoryfastaccess.h>
29 #include <alproxies/allauncherproxy.h>
30 #include <alproxies/almemoryproxy.h>
31 #include <alproxies/dcmproxy.h>
32 #include <interfaces/LedInterface.h>
33 #include <interfaces/NaoJointPositionInterface.h>
34 #include <utils/system/pathparser.h>
57 LED_EARS_RIGHT_108DEG,
58 LED_EARS_RIGHT_144DEG,
59 LED_EARS_RIGHT_180DEG,
60 LED_EARS_RIGHT_216DEG,
61 LED_EARS_RIGHT_252DEG,
62 LED_EARS_RIGHT_288DEG,
63 LED_EARS_RIGHT_324DEG,
64 LED_FACE_LEFT_RED_0DEG,
65 LED_FACE_LEFT_RED_45DEG,
66 LED_FACE_LEFT_RED_90DEG,
67 LED_FACE_LEFT_RED_135DEG,
68 LED_FACE_LEFT_RED_180DEG,
69 LED_FACE_LEFT_RED_225DEG,
70 LED_FACE_LEFT_RED_270DEG,
71 LED_FACE_LEFT_RED_315DEG,
72 LED_FACE_LEFT_GREEN_0DEG,
73 LED_FACE_LEFT_GREEN_45DEG,
74 LED_FACE_LEFT_GREEN_90DEG,
75 LED_FACE_LEFT_GREEN_135DEG,
76 LED_FACE_LEFT_GREEN_180DEG,
77 LED_FACE_LEFT_GREEN_225DEG,
78 LED_FACE_LEFT_GREEN_270DEG,
79 LED_FACE_LEFT_GREEN_315DEG,
80 LED_FACE_LEFT_BLUE_0DEG,
81 LED_FACE_LEFT_BLUE_45DEG,
82 LED_FACE_LEFT_BLUE_90DEG,
83 LED_FACE_LEFT_BLUE_135DEG,
84 LED_FACE_LEFT_BLUE_180DEG,
85 LED_FACE_LEFT_BLUE_225DEG,
86 LED_FACE_LEFT_BLUE_270DEG,
87 LED_FACE_LEFT_BLUE_315DEG,
88 LED_FACE_RIGHT_RED_0DEG,
89 LED_FACE_RIGHT_RED_45DEG,
90 LED_FACE_RIGHT_RED_90DEG,
91 LED_FACE_RIGHT_RED_135DEG,
92 LED_FACE_RIGHT_RED_180DEG,
93 LED_FACE_RIGHT_RED_225DEG,
94 LED_FACE_RIGHT_RED_270DEG,
95 LED_FACE_RIGHT_RED_315DEG,
96 LED_FACE_RIGHT_GREEN_0DEG,
97 LED_FACE_RIGHT_GREEN_45DEG,
98 LED_FACE_RIGHT_GREEN_90DEG,
99 LED_FACE_RIGHT_GREEN_135DEG,
100 LED_FACE_RIGHT_GREEN_180DEG,
101 LED_FACE_RIGHT_GREEN_225DEG,
102 LED_FACE_RIGHT_GREEN_270DEG,
103 LED_FACE_RIGHT_GREEN_315DEG,
104 LED_FACE_RIGHT_BLUE_0DEG,
105 LED_FACE_RIGHT_BLUE_45DEG,
106 LED_FACE_RIGHT_BLUE_90DEG,
107 LED_FACE_RIGHT_BLUE_135DEG,
108 LED_FACE_RIGHT_BLUE_180DEG,
109 LED_FACE_RIGHT_BLUE_225DEG,
110 LED_FACE_RIGHT_BLUE_270DEG,
111 LED_FACE_RIGHT_BLUE_315DEG,
131 :
Thread(
"NaoQiLedThread",
Thread::OPMODE_WAITFORWAKEUP),
145 cfg_verbose_face_ =
false;
147 cfg_verbose_face_ =
config->
get_bool(
"/hardware/nao/leds/verbose_face");
155 subd_prefix_ = (std::string)dcm_->getPrefix()[0];
156 }
catch (AL::ALError &e) {
157 throw Exception(
"Failed to get DCM prefix: %s", e.toString().c_str());
161 std::vector<std::string> leddevs;
163 leddevs = dcm::get_devices(dcm_, almem_,
"Led");
164 }
catch (AL::ALError &e) {
165 throw Exception(
"Failed to get LED devices: %s", e.toString().c_str());
169 std::string prefix = subd_prefix_;
170 std::vector<std::string> keys;
171 keys.resize(LedTypeN);
172 values_.resize(LedTypeN);
174 keys[LED_CHESTBOARD_RED] = prefix +
"ChestBoard/Led/Red/Actuator/Value";
175 keys[LED_CHESTBOARD_GREEN] = prefix +
"ChestBoard/Led/Green/Actuator/Value";
176 keys[LED_CHESTBOARD_BLUE] = prefix +
"ChestBoard/Led/Blue/Actuator/Value";
178 prefix = subd_prefix_ +
"Ears/Led/";
179 keys[LED_EARS_LEFT_0DEG] = prefix +
"Left/0Deg/Actuator/Value";
180 keys[LED_EARS_LEFT_36DEG] = prefix +
"Left/36Deg/Actuator/Value";
181 keys[LED_EARS_LEFT_72DEG] = prefix +
"Left/72Deg/Actuator/Value";
182 keys[LED_EARS_LEFT_108DEG] = prefix +
"Left/108Deg/Actuator/Value";
183 keys[LED_EARS_LEFT_144DEG] = prefix +
"Left/144Deg/Actuator/Value";
184 keys[LED_EARS_LEFT_180DEG] = prefix +
"Left/180Deg/Actuator/Value";
185 keys[LED_EARS_LEFT_216DEG] = prefix +
"Left/216Deg/Actuator/Value";
186 keys[LED_EARS_LEFT_252DEG] = prefix +
"Left/252Deg/Actuator/Value";
187 keys[LED_EARS_LEFT_288DEG] = prefix +
"Left/288Deg/Actuator/Value";
188 keys[LED_EARS_LEFT_324DEG] = prefix +
"Left/324Deg/Actuator/Value";
190 keys[LED_EARS_RIGHT_0DEG] = prefix +
"Right/0Deg/Actuator/Value";
191 keys[LED_EARS_RIGHT_36DEG] = prefix +
"Right/36Deg/Actuator/Value";
192 keys[LED_EARS_RIGHT_72DEG] = prefix +
"Right/72Deg/Actuator/Value";
193 keys[LED_EARS_RIGHT_108DEG] = prefix +
"Right/108Deg/Actuator/Value";
194 keys[LED_EARS_RIGHT_144DEG] = prefix +
"Right/144Deg/Actuator/Value";
195 keys[LED_EARS_RIGHT_180DEG] = prefix +
"Right/180Deg/Actuator/Value";
196 keys[LED_EARS_RIGHT_216DEG] = prefix +
"Right/216Deg/Actuator/Value";
197 keys[LED_EARS_RIGHT_252DEG] = prefix +
"Right/252Deg/Actuator/Value";
198 keys[LED_EARS_RIGHT_288DEG] = prefix +
"Right/288Deg/Actuator/Value";
199 keys[LED_EARS_RIGHT_324DEG] = prefix +
"Right/324Deg/Actuator/Value";
201 prefix = subd_prefix_ +
"Face/Led/";
202 keys[LED_FACE_LEFT_RED_0DEG] = prefix +
"Red/Left/0Deg/Actuator/Value";
203 keys[LED_FACE_LEFT_RED_45DEG] = prefix +
"Red/Left/45Deg/Actuator/Value";
204 keys[LED_FACE_LEFT_RED_90DEG] = prefix +
"Red/Left/90Deg/Actuator/Value";
205 keys[LED_FACE_LEFT_RED_135DEG] = prefix +
"Red/Left/135Deg/Actuator/Value";
206 keys[LED_FACE_LEFT_RED_180DEG] = prefix +
"Red/Left/180Deg/Actuator/Value";
207 keys[LED_FACE_LEFT_RED_225DEG] = prefix +
"Red/Left/225Deg/Actuator/Value";
208 keys[LED_FACE_LEFT_RED_270DEG] = prefix +
"Red/Left/270Deg/Actuator/Value";
209 keys[LED_FACE_LEFT_RED_315DEG] = prefix +
"Red/Left/315Deg/Actuator/Value";
211 keys[LED_FACE_LEFT_GREEN_0DEG] = prefix +
"Green/Left/0Deg/Actuator/Value";
212 keys[LED_FACE_LEFT_GREEN_45DEG] = prefix +
"Green/Left/45Deg/Actuator/Value";
213 keys[LED_FACE_LEFT_GREEN_90DEG] = prefix +
"Green/Left/90Deg/Actuator/Value";
214 keys[LED_FACE_LEFT_GREEN_135DEG] = prefix +
"Green/Left/135Deg/Actuator/Value";
215 keys[LED_FACE_LEFT_GREEN_180DEG] = prefix +
"Green/Left/180Deg/Actuator/Value";
216 keys[LED_FACE_LEFT_GREEN_225DEG] = prefix +
"Green/Left/225Deg/Actuator/Value";
217 keys[LED_FACE_LEFT_GREEN_270DEG] = prefix +
"Green/Left/270Deg/Actuator/Value";
218 keys[LED_FACE_LEFT_GREEN_315DEG] = prefix +
"Green/Left/315Deg/Actuator/Value";
220 keys[LED_FACE_LEFT_BLUE_0DEG] = prefix +
"Blue/Left/0Deg/Actuator/Value";
221 keys[LED_FACE_LEFT_BLUE_45DEG] = prefix +
"Blue/Left/45Deg/Actuator/Value";
222 keys[LED_FACE_LEFT_BLUE_90DEG] = prefix +
"Blue/Left/90Deg/Actuator/Value";
223 keys[LED_FACE_LEFT_BLUE_135DEG] = prefix +
"Blue/Left/135Deg/Actuator/Value";
224 keys[LED_FACE_LEFT_BLUE_180DEG] = prefix +
"Blue/Left/180Deg/Actuator/Value";
225 keys[LED_FACE_LEFT_BLUE_225DEG] = prefix +
"Blue/Left/225Deg/Actuator/Value";
226 keys[LED_FACE_LEFT_BLUE_270DEG] = prefix +
"Blue/Left/270Deg/Actuator/Value";
227 keys[LED_FACE_LEFT_BLUE_315DEG] = prefix +
"Blue/Left/315Deg/Actuator/Value";
229 keys[LED_FACE_RIGHT_RED_0DEG] = prefix +
"Red/Right/0Deg/Actuator/Value";
230 keys[LED_FACE_RIGHT_RED_45DEG] = prefix +
"Red/Right/45Deg/Actuator/Value";
231 keys[LED_FACE_RIGHT_RED_90DEG] = prefix +
"Red/Right/90Deg/Actuator/Value";
232 keys[LED_FACE_RIGHT_RED_135DEG] = prefix +
"Red/Right/135Deg/Actuator/Value";
233 keys[LED_FACE_RIGHT_RED_180DEG] = prefix +
"Red/Right/180Deg/Actuator/Value";
234 keys[LED_FACE_RIGHT_RED_225DEG] = prefix +
"Red/Right/225Deg/Actuator/Value";
235 keys[LED_FACE_RIGHT_RED_270DEG] = prefix +
"Red/Right/270Deg/Actuator/Value";
236 keys[LED_FACE_RIGHT_RED_315DEG] = prefix +
"Red/Right/315Deg/Actuator/Value";
238 keys[LED_FACE_RIGHT_GREEN_0DEG] = prefix +
"Green/Right/0Deg/Actuator/Value";
239 keys[LED_FACE_RIGHT_GREEN_45DEG] = prefix +
"Green/Right/45Deg/Actuator/Value";
240 keys[LED_FACE_RIGHT_GREEN_90DEG] = prefix +
"Green/Right/90Deg/Actuator/Value";
241 keys[LED_FACE_RIGHT_GREEN_135DEG] = prefix +
"Green/Right/135Deg/Actuator/Value";
242 keys[LED_FACE_RIGHT_GREEN_180DEG] = prefix +
"Green/Right/180Deg/Actuator/Value";
243 keys[LED_FACE_RIGHT_GREEN_225DEG] = prefix +
"Green/Right/225Deg/Actuator/Value";
244 keys[LED_FACE_RIGHT_GREEN_270DEG] = prefix +
"Green/Right/270Deg/Actuator/Value";
245 keys[LED_FACE_RIGHT_GREEN_315DEG] = prefix +
"Green/Right/315Deg/Actuator/Value";
247 keys[LED_FACE_RIGHT_BLUE_0DEG] = prefix +
"Blue/Right/0Deg/Actuator/Value";
248 keys[LED_FACE_RIGHT_BLUE_45DEG] = prefix +
"Blue/Right/45Deg/Actuator/Value";
249 keys[LED_FACE_RIGHT_BLUE_90DEG] = prefix +
"Blue/Right/90Deg/Actuator/Value";
250 keys[LED_FACE_RIGHT_BLUE_135DEG] = prefix +
"Blue/Right/135Deg/Actuator/Value";
251 keys[LED_FACE_RIGHT_BLUE_180DEG] = prefix +
"Blue/Right/180Deg/Actuator/Value";
252 keys[LED_FACE_RIGHT_BLUE_225DEG] = prefix +
"Blue/Right/225Deg/Actuator/Value";
253 keys[LED_FACE_RIGHT_BLUE_270DEG] = prefix +
"Blue/Right/270Deg/Actuator/Value";
254 keys[LED_FACE_RIGHT_BLUE_315DEG] = prefix +
"Blue/Right/315Deg/Actuator/Value";
256 prefix = subd_prefix_;
257 keys[LED_LFOOT_RED] = prefix +
"LFoot/Led/Red/Actuator/Value";
258 keys[LED_LFOOT_GREEN] = prefix +
"LFoot/Led/Green/Actuator/Value";
259 keys[LED_LFOOT_BLUE] = prefix +
"LFoot/Led/Blue/Actuator/Value";
261 keys[LED_RFOOT_RED] = prefix +
"RFoot/Led/Red/Actuator/Value";
262 keys[LED_RFOOT_GREEN] = prefix +
"RFoot/Led/Green/Actuator/Value";
263 keys[LED_RFOOT_BLUE] = prefix +
"RFoot/Led/Blue/Actuator/Value";
265 memfa_.reset(
new AL::ALMemoryFastAccess());
268 }
catch (AL::ALError &e) {
269 throw Exception(
"Failed to setup fast memory access: %s", e.toString().c_str());
276 throw Exception(
"Joint Position interface has no writer");
278 joint_pos_if->
read();
279 bool skip_head_leds =
280 (joint_pos_if->
robot_type() != NaoJointPositionInterface::ROBOTYPE_ACADEMIC);
283 std::vector<std::string>::iterator l;
284 for (l = leddevs.begin(); l != leddevs.end(); ++l) {
286 std::string loc = pp[subdpp.size()];
288 if (!cfg_verbose_face_) {
290 if (locpp[0] ==
"Face")
293 if (skip_head_leds) {
295 if (locpp[0] ==
"Head")
299 std::string
id =
"Nao LED " + loc;
300 PathParser::size_type i;
301 for (i = subdpp.size() + 2; (i < pp.size()) && (pp[i] !=
"Actuator"); ++i) {
308 leds_.insert(make_pair(iface, *l +
"/Value"));
311 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
312 if (i->first != last) {
323 std::string left_right[2] = {
"Left",
"Right"};
324 std::string rgb[3] = {
"Red",
"Green",
"Blue"};
325 std::string angles[8] = {
"0",
"45",
"90",
"135",
"180",
"225",
"270",
"315"};
327 for (
unsigned int lr = 0; lr < 2; ++lr) {
328 for (
unsigned int cl = 0; cl < 3; ++cl) {
329 std::string
id =
"Nao LED Face/" + rgb[cl] +
"/" + left_right[lr];
332 for (
unsigned int a = 0; a < 8; ++a) {
333 std::string entry =
"Face/Led/" + rgb[cl] +
"/" + left_right[lr];
334 std::string memid = subd_prefix_ + entry +
"/" + angles[a] +
"Deg/Actuator/Value";
336 leds_.insert(make_pair(iface, memid));
343 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
344 if (i->first != last) {
355 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
356 if (i->first == last)
360 std::pair<LedMap::iterator, LedMap::iterator> ret = leds_.equal_range(i->first);
362 for (LedMap::iterator j = ret.first; j != ret.second; ++j) {
365 for (
unsigned int k = 0; k < keys.size(); ++k) {
366 if (keys[k] == j->second) {
367 memids_.insert(std::make_pair(i->first, k));
377 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
378 if (i->first != last) {
392 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
393 if (i->first != last) {
407 memfa_->GetValues(values_);
410 for (LedMap::iterator i = leds_.begin(); i != leds_.end(); ++i) {
411 if (i->first == last)
416 std::pair<LedMemMap::iterator, LedMemMap::iterator> ret = memids_.equal_range(i->first);
417 for (LedMemMap::iterator j = ret.first; j != ret.second; ++j) {
418 if (values_[j->second] > maxval)
419 maxval = values_[j->second];
422 if (maxval != i->first->intensity()) {
423 i->first->set_intensity(maxval);
435 std::string kind =
"Merge";
436 int dcm_time = dcm_->getTime(0);
445 std::pair<LedMap::iterator, LedMap::iterator> ret = leds_.equal_range(led_if);
448 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
449 printf(
"Set %s to %f\n", i->second.c_str(), sim->
intensity());
451 dcm_, i->second, kind, sim->
intensity(), (
int)roundf(dcm_time + sim->
time_sec() * 1000.));
454 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
455 dcm::set_value(dcm_, i->second, kind, 1., dcm_time);
458 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
459 dcm::set_value(dcm_, i->second, kind, 0., dcm_time);