23 #include "led_thread.h" 24 #include "dcm_utils.h" 26 #include <utils/system/pathparser.h> 28 #include <alproxies/allauncherproxy.h> 29 #include <alproxies/dcmproxy.h> 30 #include <alproxies/almemoryproxy.h> 31 #include <alcore/alerror.h> 32 #include <almemoryfastaccess/almemoryfastaccess.h> 34 #include <interfaces/LedInterface.h> 35 #include <interfaces/NaoJointPositionInterface.h> 42 LED_CHESTBOARD_RED, LED_CHESTBOARD_GREEN, LED_CHESTBOARD_BLUE,
43 LED_EARS_LEFT_0DEG, LED_EARS_LEFT_36DEG, LED_EARS_LEFT_72DEG,
44 LED_EARS_LEFT_108DEG, LED_EARS_LEFT_144DEG, LED_EARS_LEFT_180DEG,
45 LED_EARS_LEFT_216DEG, LED_EARS_LEFT_252DEG, LED_EARS_LEFT_288DEG,
47 LED_EARS_RIGHT_0DEG, LED_EARS_RIGHT_36DEG, LED_EARS_RIGHT_72DEG,
48 LED_EARS_RIGHT_108DEG, LED_EARS_RIGHT_144DEG, LED_EARS_RIGHT_180DEG,
49 LED_EARS_RIGHT_216DEG, LED_EARS_RIGHT_252DEG, LED_EARS_RIGHT_288DEG,
50 LED_EARS_RIGHT_324DEG,
51 LED_FACE_LEFT_RED_0DEG, LED_FACE_LEFT_RED_45DEG,
52 LED_FACE_LEFT_RED_90DEG, LED_FACE_LEFT_RED_135DEG, LED_FACE_LEFT_RED_180DEG,
53 LED_FACE_LEFT_RED_225DEG, LED_FACE_LEFT_RED_270DEG, LED_FACE_LEFT_RED_315DEG,
54 LED_FACE_LEFT_GREEN_0DEG, LED_FACE_LEFT_GREEN_45DEG, LED_FACE_LEFT_GREEN_90DEG,
55 LED_FACE_LEFT_GREEN_135DEG, LED_FACE_LEFT_GREEN_180DEG,
56 LED_FACE_LEFT_GREEN_225DEG, LED_FACE_LEFT_GREEN_270DEG,
57 LED_FACE_LEFT_GREEN_315DEG,
58 LED_FACE_LEFT_BLUE_0DEG, LED_FACE_LEFT_BLUE_45DEG,
59 LED_FACE_LEFT_BLUE_90DEG, LED_FACE_LEFT_BLUE_135DEG,
60 LED_FACE_LEFT_BLUE_180DEG, LED_FACE_LEFT_BLUE_225DEG,
61 LED_FACE_LEFT_BLUE_270DEG, LED_FACE_LEFT_BLUE_315DEG,
62 LED_FACE_RIGHT_RED_0DEG, LED_FACE_RIGHT_RED_45DEG, LED_FACE_RIGHT_RED_90DEG,
63 LED_FACE_RIGHT_RED_135DEG, LED_FACE_RIGHT_RED_180DEG, LED_FACE_RIGHT_RED_225DEG,
64 LED_FACE_RIGHT_RED_270DEG, LED_FACE_RIGHT_RED_315DEG,
65 LED_FACE_RIGHT_GREEN_0DEG, LED_FACE_RIGHT_GREEN_45DEG, LED_FACE_RIGHT_GREEN_90DEG,
66 LED_FACE_RIGHT_GREEN_135DEG, LED_FACE_RIGHT_GREEN_180DEG,
67 LED_FACE_RIGHT_GREEN_225DEG,
68 LED_FACE_RIGHT_GREEN_270DEG, LED_FACE_RIGHT_GREEN_315DEG,
69 LED_FACE_RIGHT_BLUE_0DEG, LED_FACE_RIGHT_BLUE_45DEG,
70 LED_FACE_RIGHT_BLUE_90DEG, LED_FACE_RIGHT_BLUE_135DEG,
71 LED_FACE_RIGHT_BLUE_180DEG, LED_FACE_RIGHT_BLUE_225DEG,
72 LED_FACE_RIGHT_BLUE_270DEG, LED_FACE_RIGHT_BLUE_315DEG,
73 LED_LFOOT_RED, LED_LFOOT_GREEN, LED_LFOOT_BLUE,
74 LED_RFOOT_RED, LED_RFOOT_GREEN, LED_RFOOT_BLUE,
89 :
Thread(
"NaoQiLedThread",
Thread::OPMODE_WAITFORWAKEUP),
104 __cfg_verbose_face =
false;
106 __cfg_verbose_face =
config->
get_bool(
"/hardware/nao/leds/verbose_face");
113 __subd_prefix = (std::string)__dcm->getPrefix()[0];
114 }
catch (AL::ALError &e) {
115 throw Exception(
"Failed to get DCM prefix: %s", e.toString().c_str());
119 std::vector<std::string> leddevs;
121 leddevs = dcm::get_devices(__dcm, __almem,
"Led");
122 }
catch (AL::ALError &e) {
123 throw Exception(
"Failed to get LED devices: %s", e.toString().c_str());
127 std::string prefix = __subd_prefix;
128 std::vector<std::string> keys;
129 keys.resize(LedTypeN);
130 __values.resize(LedTypeN);
132 keys[LED_CHESTBOARD_RED] = prefix +
"ChestBoard/Led/Red/Actuator/Value";
133 keys[LED_CHESTBOARD_GREEN] = prefix +
"ChestBoard/Led/Green/Actuator/Value";
134 keys[LED_CHESTBOARD_BLUE] = prefix +
"ChestBoard/Led/Blue/Actuator/Value";
136 prefix = __subd_prefix +
"Ears/Led/";
137 keys[LED_EARS_LEFT_0DEG] = prefix +
"Left/0Deg/Actuator/Value";
138 keys[LED_EARS_LEFT_36DEG] = prefix +
"Left/36Deg/Actuator/Value";
139 keys[LED_EARS_LEFT_72DEG] = prefix +
"Left/72Deg/Actuator/Value";
140 keys[LED_EARS_LEFT_108DEG] = prefix +
"Left/108Deg/Actuator/Value";
141 keys[LED_EARS_LEFT_144DEG] = prefix +
"Left/144Deg/Actuator/Value";
142 keys[LED_EARS_LEFT_180DEG] = prefix +
"Left/180Deg/Actuator/Value";
143 keys[LED_EARS_LEFT_216DEG] = prefix +
"Left/216Deg/Actuator/Value";
144 keys[LED_EARS_LEFT_252DEG] = prefix +
"Left/252Deg/Actuator/Value";
145 keys[LED_EARS_LEFT_288DEG] = prefix +
"Left/288Deg/Actuator/Value";
146 keys[LED_EARS_LEFT_324DEG] = prefix +
"Left/324Deg/Actuator/Value";
148 keys[LED_EARS_RIGHT_0DEG] = prefix +
"Right/0Deg/Actuator/Value";
149 keys[LED_EARS_RIGHT_36DEG] = prefix +
"Right/36Deg/Actuator/Value";
150 keys[LED_EARS_RIGHT_72DEG] = prefix +
"Right/72Deg/Actuator/Value";
151 keys[LED_EARS_RIGHT_108DEG] = prefix +
"Right/108Deg/Actuator/Value";
152 keys[LED_EARS_RIGHT_144DEG] = prefix +
"Right/144Deg/Actuator/Value";
153 keys[LED_EARS_RIGHT_180DEG] = prefix +
"Right/180Deg/Actuator/Value";
154 keys[LED_EARS_RIGHT_216DEG] = prefix +
"Right/216Deg/Actuator/Value";
155 keys[LED_EARS_RIGHT_252DEG] = prefix +
"Right/252Deg/Actuator/Value";
156 keys[LED_EARS_RIGHT_288DEG] = prefix +
"Right/288Deg/Actuator/Value";
157 keys[LED_EARS_RIGHT_324DEG] = prefix +
"Right/324Deg/Actuator/Value";
159 prefix = __subd_prefix +
"Face/Led/";
160 keys[LED_FACE_LEFT_RED_0DEG] = prefix +
"Red/Left/0Deg/Actuator/Value";
161 keys[LED_FACE_LEFT_RED_45DEG] = prefix +
"Red/Left/45Deg/Actuator/Value";
162 keys[LED_FACE_LEFT_RED_90DEG] = prefix +
"Red/Left/90Deg/Actuator/Value";
163 keys[LED_FACE_LEFT_RED_135DEG] = prefix +
"Red/Left/135Deg/Actuator/Value";
164 keys[LED_FACE_LEFT_RED_180DEG] = prefix +
"Red/Left/180Deg/Actuator/Value";
165 keys[LED_FACE_LEFT_RED_225DEG] = prefix +
"Red/Left/225Deg/Actuator/Value";
166 keys[LED_FACE_LEFT_RED_270DEG] = prefix +
"Red/Left/270Deg/Actuator/Value";
167 keys[LED_FACE_LEFT_RED_315DEG] = prefix +
"Red/Left/315Deg/Actuator/Value";
169 keys[LED_FACE_LEFT_GREEN_0DEG] = prefix +
"Green/Left/0Deg/Actuator/Value";
170 keys[LED_FACE_LEFT_GREEN_45DEG] = prefix +
"Green/Left/45Deg/Actuator/Value";
171 keys[LED_FACE_LEFT_GREEN_90DEG] = prefix +
"Green/Left/90Deg/Actuator/Value";
172 keys[LED_FACE_LEFT_GREEN_135DEG] = prefix +
"Green/Left/135Deg/Actuator/Value";
173 keys[LED_FACE_LEFT_GREEN_180DEG] = prefix +
"Green/Left/180Deg/Actuator/Value";
174 keys[LED_FACE_LEFT_GREEN_225DEG] = prefix +
"Green/Left/225Deg/Actuator/Value";
175 keys[LED_FACE_LEFT_GREEN_270DEG] = prefix +
"Green/Left/270Deg/Actuator/Value";
176 keys[LED_FACE_LEFT_GREEN_315DEG] = prefix +
"Green/Left/315Deg/Actuator/Value";
178 keys[LED_FACE_LEFT_BLUE_0DEG] = prefix +
"Blue/Left/0Deg/Actuator/Value";
179 keys[LED_FACE_LEFT_BLUE_45DEG] = prefix +
"Blue/Left/45Deg/Actuator/Value";
180 keys[LED_FACE_LEFT_BLUE_90DEG] = prefix +
"Blue/Left/90Deg/Actuator/Value";
181 keys[LED_FACE_LEFT_BLUE_135DEG] = prefix +
"Blue/Left/135Deg/Actuator/Value";
182 keys[LED_FACE_LEFT_BLUE_180DEG] = prefix +
"Blue/Left/180Deg/Actuator/Value";
183 keys[LED_FACE_LEFT_BLUE_225DEG] = prefix +
"Blue/Left/225Deg/Actuator/Value";
184 keys[LED_FACE_LEFT_BLUE_270DEG] = prefix +
"Blue/Left/270Deg/Actuator/Value";
185 keys[LED_FACE_LEFT_BLUE_315DEG] = prefix +
"Blue/Left/315Deg/Actuator/Value";
188 keys[LED_FACE_RIGHT_RED_0DEG] = prefix +
"Red/Right/0Deg/Actuator/Value";
189 keys[LED_FACE_RIGHT_RED_45DEG] = prefix +
"Red/Right/45Deg/Actuator/Value";
190 keys[LED_FACE_RIGHT_RED_90DEG] = prefix +
"Red/Right/90Deg/Actuator/Value";
191 keys[LED_FACE_RIGHT_RED_135DEG] = prefix +
"Red/Right/135Deg/Actuator/Value";
192 keys[LED_FACE_RIGHT_RED_180DEG] = prefix +
"Red/Right/180Deg/Actuator/Value";
193 keys[LED_FACE_RIGHT_RED_225DEG] = prefix +
"Red/Right/225Deg/Actuator/Value";
194 keys[LED_FACE_RIGHT_RED_270DEG] = prefix +
"Red/Right/270Deg/Actuator/Value";
195 keys[LED_FACE_RIGHT_RED_315DEG] = prefix +
"Red/Right/315Deg/Actuator/Value";
197 keys[LED_FACE_RIGHT_GREEN_0DEG] = prefix +
"Green/Right/0Deg/Actuator/Value";
198 keys[LED_FACE_RIGHT_GREEN_45DEG] = prefix +
"Green/Right/45Deg/Actuator/Value";
199 keys[LED_FACE_RIGHT_GREEN_90DEG] = prefix +
"Green/Right/90Deg/Actuator/Value";
200 keys[LED_FACE_RIGHT_GREEN_135DEG] = prefix +
"Green/Right/135Deg/Actuator/Value";
201 keys[LED_FACE_RIGHT_GREEN_180DEG] = prefix +
"Green/Right/180Deg/Actuator/Value";
202 keys[LED_FACE_RIGHT_GREEN_225DEG] = prefix +
"Green/Right/225Deg/Actuator/Value";
203 keys[LED_FACE_RIGHT_GREEN_270DEG] = prefix +
"Green/Right/270Deg/Actuator/Value";
204 keys[LED_FACE_RIGHT_GREEN_315DEG] = prefix +
"Green/Right/315Deg/Actuator/Value";
206 keys[LED_FACE_RIGHT_BLUE_0DEG] = prefix +
"Blue/Right/0Deg/Actuator/Value";
207 keys[LED_FACE_RIGHT_BLUE_45DEG] = prefix +
"Blue/Right/45Deg/Actuator/Value";
208 keys[LED_FACE_RIGHT_BLUE_90DEG] = prefix +
"Blue/Right/90Deg/Actuator/Value";
209 keys[LED_FACE_RIGHT_BLUE_135DEG] = prefix +
"Blue/Right/135Deg/Actuator/Value";
210 keys[LED_FACE_RIGHT_BLUE_180DEG] = prefix +
"Blue/Right/180Deg/Actuator/Value";
211 keys[LED_FACE_RIGHT_BLUE_225DEG] = prefix +
"Blue/Right/225Deg/Actuator/Value";
212 keys[LED_FACE_RIGHT_BLUE_270DEG] = prefix +
"Blue/Right/270Deg/Actuator/Value";
213 keys[LED_FACE_RIGHT_BLUE_315DEG] = prefix +
"Blue/Right/315Deg/Actuator/Value";
216 prefix = __subd_prefix;
217 keys[LED_LFOOT_RED] = prefix +
"LFoot/Led/Red/Actuator/Value";
218 keys[LED_LFOOT_GREEN] = prefix +
"LFoot/Led/Green/Actuator/Value";
219 keys[LED_LFOOT_BLUE] = prefix +
"LFoot/Led/Blue/Actuator/Value";
221 keys[LED_RFOOT_RED] = prefix +
"RFoot/Led/Red/Actuator/Value";
222 keys[LED_RFOOT_GREEN] = prefix +
"RFoot/Led/Green/Actuator/Value";
223 keys[LED_RFOOT_BLUE] = prefix +
"RFoot/Led/Blue/Actuator/Value";
225 __memfa.reset(
new AL::ALMemoryFastAccess());
228 }
catch (AL::ALError &e) {
229 throw Exception(
"Failed to setup fast memory access: %s",
230 e.toString().c_str());
235 if (! joint_pos_if->has_writer()) {
237 throw Exception(
"Joint Position interface has no writer");
239 joint_pos_if->read();
240 bool skip_head_leds =
241 (joint_pos_if->robot_type() != NaoJointPositionInterface::ROBOTYPE_ACADEMIC);
244 std::vector<std::string>::iterator l;
245 for (l = leddevs.begin(); l != leddevs.end(); ++l) {
247 std::string loc = pp[subdpp.size()];
249 if (! __cfg_verbose_face) {
251 if (locpp[0] ==
"Face")
continue;
253 if (skip_head_leds) {
255 if (locpp[0] ==
"Head")
continue;
258 std::string
id =
"Nao LED " + loc;
259 PathParser::size_type i;
260 for (i = subdpp.size() + 2; (i < pp.size()) && (pp[i] !=
"Actuator"); ++i) {
268 __leds.insert(make_pair(iface, *l +
"/Value"));
271 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
272 if (i->first != last) {
283 std::string left_right[2] = {
"Left",
"Right" };
284 std::string rgb[3] = {
"Red",
"Green",
"Blue" };
285 std::string angles[8] = {
"0",
"45",
"90",
"135",
"180",
"225",
"270",
"315"};
287 for (
unsigned int lr = 0; lr < 2; ++lr) {
288 for (
unsigned int cl = 0; cl < 3; ++cl) {
289 std::string
id =
"Nao LED Face/" + rgb[cl] +
"/" + left_right[lr];
293 for (
unsigned int a = 0; a < 8; ++a) {
294 std::string entry =
"Face/Led/" + rgb[cl] +
"/" + left_right[lr];
296 __subd_prefix + entry +
"/" + angles[a] +
"Deg/Actuator/Value";
298 __leds.insert(make_pair(iface, memid));
305 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
306 if (i->first != last) {
317 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
318 if (i->first == last)
continue;
321 std::pair<LedMap::iterator, LedMap::iterator> ret =
322 __leds.equal_range(i->first);
324 for (LedMap::iterator j = ret.first; j != ret.second; ++j) {
327 for (
unsigned int k = 0; k < keys.size(); ++k) {
328 if (keys[k] == j->second) {
329 __memids.insert(std::make_pair(i->first, k));
339 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
340 if (i->first != last) {
354 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
355 if (i->first != last) {
369 __memfa->GetValues(__values);
372 for (LedMap::iterator i = __leds.begin(); i != __leds.end(); ++i) {
373 if (i->first == last)
continue;
377 std::pair<LedMemMap::iterator, LedMemMap::iterator> ret =
378 __memids.equal_range(i->first);
379 for (LedMemMap::iterator j = ret.first; j != ret.second; ++j) {
380 if (__values[j->second] > maxval) maxval = __values[j->second];
383 if (maxval != i->first->intensity()) {
384 i->first->set_intensity(maxval);
398 std::string kind =
"Merge";
399 int dcm_time = __dcm->getTime(0);
405 if (led_if == NULL)
return false;
407 std::pair<LedMap::iterator, LedMap::iterator> ret =
408 __leds.equal_range(led_if);
411 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
412 printf(
"Set %s to %f\n", i->second.c_str(), sim->
intensity());
413 dcm::set_value(__dcm, i->second, kind, sim->
intensity(),
414 (int)roundf(dcm_time + sim->
time_sec() * 1000.));
416 }
else if (dynamic_cast<LedInterface::TurnOnMessage *>(message) != NULL) {
417 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
418 dcm::set_value(__dcm, i->second, kind, 1., dcm_time);
420 }
else if (dynamic_cast<LedInterface::TurnOffMessage *>(message) != NULL) {
421 for (LedMap::iterator i = ret.first; i != ret.second; ++i) {
422 dcm::set_value(__dcm, i->second, kind, 0., dcm_time);
virtual void finalize()
Finalize the thread.
NaoJointPositionInterface Fawkes BlackBoard Interface.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
AL::ALPtr< AL::ALBroker > naoqi_broker
NaoQi broker.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Thread class encapsulation of pthreads.
Base class for all Fawkes BlackBoard interfaces.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
virtual void init()
Initialize the thread.
Thread aspect to use blocked timing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
virtual void loop()
Code to execute in the thread.
Base class for exceptions in Fawkes.
SetIntensityMessage Fawkes BlackBoard Interface Message.
bool bb_interface_message_received(fawkes::Interface *interface, fawkes::Message *message)
BlackBoard message received notification.
LedInterface Fawkes BlackBoard Interface.
float intensity() const
Get intensity value.
NaoQiLedThread()
Constructor.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
float time_sec() const
Get time_sec value.
virtual ~NaoQiLedThread()
Destructor.
Configuration * config
This is the Configuration member used to access the configuration.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
BlackBoard interface listener.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void close(Interface *interface)=0
Close interface.